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Abstract 


When trying to fly an aircraft as smoothly as possible it is a good idea to 
use the derivatives of the pilot command instead of using the actual control. 
This idea was implemented with splines and control theory, in a system that 
tries to model an aircraft. Computer calculations in Matlab shows that it is 
impossible to receive enough smooth control signals by this way. This is due 
to the fact that the splines not only try to approximate the test function, 
but also its derivatives. A perfect traction is received but we have to pay in 
very peaky control signals and accelerations. 
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2 Introduction 

In the beginning our intention was to calculate control laws for an aircraft 
model so it would fly as smooth as possible in three dimensions. The comfort 
for the passengers was the most important consideration when we forced our 
system, the representation of the plane, to follow a certain trajectory. 

All the programming has been realized in a numeric computation and 
visualization software called Matlab. Also Maple has been used in some 
of the heaviest calculations and my third contact with the more advanced 
computer world was Latex that this report is written in. The second half of 
this paper consists of Matlab code ended with listed references. 

The test began in one dimension with three different kinds of systems. 
By way of introduction the essential conceptions of reachability and stability 
were examined and written down in chapter 3 and 4 respectively. With these 
tools we could investigate the main features of the systems and obtained the 
result that all of them were completely reachable, stable but not guaranteed 
input-output stable (see chapter 5, The Systems). 

A spline is the curve of an n-degree polynomial that is joined in its end- 
points with similar polynomials. They are connected in the way that they 
have the first n-1 derivatives, at the jointly point, in common. Chapter 6 
consists of calculations for the spline approximation and the control theory. 

Chapter 8, Results, discusses some of the results we received and also 
displays examples of graphs that were obtained. The test could not be con- 
cluded in the way we thought due to a surprising combination between con- 
trol theory and splines. The last two pages in chapter 8 deals with this main 
result. 
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3 Reachability 

When controlling an aircraft we will be sure that a suitable control signal u 
can take us to all desirable states. Transfered to our one dimensional case 
we have to determine under what circumstances there is an input signal it 
which transfers the state from x(t 0 ) = x 0 to x(tj) = x f . This is a basic issue 
in systems theory and it leads to the concept of reachability. We will call a 
system completely reachable if it has the property that this can be done in 
any positive time for any two points. 

Most of the trains of thought in the following proofs are derived from 
lecture notes given by Tomas Bjork, Optimization and Systems Theory, Royal 
Institute of Technology, Stockholm, Sweden, during the fall of 93. 

Consider the system. 

x(t) = A(t)x(t) + B(t)u(t); x(t 0 ) = x 0 . 

with the general solution 

x(t) = <P{t,t 0 )x 0 + f $(t, s)B(s)u(s)ds. 

J to 

In order to reach the desired state x(^i) = x\ the following equality must be 
fulfilled. 

X; = <P(t t ,t 0 )x 0 + f $(t 1 ,s)B(s)u(s)ds. 

J to 

Define d = xj — <P(ti , to)xo and let U be the space of input signals. Defining 

the mapping L : U — + R n as 

Equation 3.1 Lu = fjj $(t t , s)B(s)u(s)ds. 

It is obvious that the desired state transfer is possible if and only if the 
equation Lu=d has a solution, i.e. d 6 Im L. 

It is easily verified that I is a linear mapping, but since it does not 
act between two finite-dimensional vector spaces, L does not have a finite- 
dimensional matrix representation. 

Taylor’s ‘Introduction to Functional Analysis’ helps us prove the following 
theorem [Taylor, page 250]. 

Theorem 3.1 If X,Y are complete inner product spaces and L : X — > Y is 
a linear continuous operator then 


Im L = Im LL 
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R is a Hilbert space but wbat kind of space is £/? Define the inner product 
for U as 

(u,z>)„= f u(t) T v(t)dt 
Jto 

and it can be proved that Li becomes a Hilbert space. The adjoint operator 
L* is determined by 

(Lu, d)ftn = d T f $(tj , s)B(s)u(s)ds = 

Jto 

J*‘ {B T {s)<P T {t I ,s)d} T u{s)ds = {u,L-d)* 

VueU,deR n 

Consequently we get 

L* '■ R n U as (L* d)(t) = B T (t)$ T (t 1 ,t)d 

and finally 

f rh 

LL'd= / <P(t 1 ,s)B(s)B T {s)‘P T (t 1 ,s)ds d 

Jto 

We thus have a linear mapping 

LL * :R n ^R n 

that is given by the symmetric, positive semidefinite n x n matrix. 

W(to,ti)= [ U *( t 1 ,s)B(s)B T (s)<P T (t 1 ,s)ds 

Jto 

Theorem 3.2 We can take a system from x(t 0 ) = x 0 to x(t t ) = x t if and 
only if 

d = Xj - <P(t , , t 0 ) z 0 € Im W(t 0 ,t,) 

We also have that the control signal u with minimum norm (energy) is given 
by 

u(t) = B T (t)$ T (t l ,t)a 
there a is just any solution to 

W(t 0 ,t,)a = d 
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Remark 1 The point with the above is that it is much easier to characterize 
Im W than Im L, because W is an ordinary matrix. 

Proof Let L be as in (3.1) 

(if) Suppose first that d £ W(t 0 ,t t ), i.e. d £ Im LET, then d = Im LIT a 
for some a £ R n . Let u = L* a and we get Lu = LL*a = d. Thus, d £ Im L 
and the state transfer is possible. 

(only if) Suppose now that the state transfer is possible, i.e. d £ Im L. 
Furthermore, suppose that d g Im W{t 0 ,ti), i.e. d £ Im LL*. This will 
give a contradiction. 

Recall that for any matrix A it holds that Im A = (ker A T ) X . Since LL" 
is a symmetric matrix, we get d 0 (ker LL*) 1 . This implies that there is a 
z 6 ker LL* , i.e. LL"z = 0, such that (z, d) R n ^ 0. But, LL* z = 0 implies 
that 0 = ( z , LL*z)ftn = (L* z, L* z) u . Hence, L*z = 0. Now the contradiction 
easily follows since 

0 ^ (z, d) R n = (z, Lu) R n = ( L’z , «)„ = 0 

The final step to prove the optimality of u. Let u be any solution of Lu=d. 
Then Lu = Lu so L(u — u) = 0 . This gives 

0 = (a, L(u — u))fln = ( L* a , u — u), = (ti, u — u). 

Hence, («, u) = (u, u). We now get by using the Cauchy-Schwartz inequality 
that 

(u,u) = (m, u) < (u, u)^ s (u, u) l ! s . 

Dividing by ( u , u) 1/2 yields that 

(u, u yt s < (u, u) l ! s . 


Hence, u is optimal. □ 
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3.1 Reachability for Time-Invariant Systems 


For a time-invariant system 

i = Ax + Bu W(t 0 ,t,)= f 1 e A(il - ,) BB T e AT{tl ~ ,) ds 

J to 

the question about reachability is radically simplified. 

Definition 3.1 Let (A,B) be a matrix pair, where A isnxn.The reachability 
matrix T is defined as 

r = [B,AB,...,A n -‘ B] 

Theorem 3.3 For all (£o,^i) such that to < ti we have 

Im W ( to , ti ) = Im r 


Proof I. Im r C Im W 

Im r C Im W <£> (her r T ) ± C (A:er W T ) X •<=> ker C ker 

Presume that a 6 ker W , i.e. Wa=0 so a T Wa = 0 and hence it follows that 

a T e A ^~^B = 0 Vse[t 0 ,tt]. 

Derivation with regard to s a couple of times and s := tj gives 
a T B = 0 . . . a T AB = 0 a T A n ~ 1 B = 0 i.e. a E ker r T . 

II. Im W C Im T 

In the same way as above we are going to prove that ker r T C ker W. 
Suppose that a T T = 0. By Cayley-Hamilton follows 

a T A k B = 0 k — 0,1,2,... 


Accordingly we have 

a T e~ A, B = £ Tra T A k B = 0 

k=o k - 

So it follows that a T W=0, i.e. Wa=0, i.e. a € ker W. □ 

Remark 2 Since Im T = Im for any interval wo see that 

in the time- invariant case the image of the reachability Gramian is inde- 
pendent of the interval (<o,^i)- However, this does not imply that the state 
transfer can occur during a fortuitous short time interval. 
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Definition 3.2 Let n be the dimension of the state space . The pair (A,B) 
is said to be completely reachable i/T has full rank, i.e. 

rank T = n 

Definition 3.3 The reachable subspace 72 is defined as 

n = Im [b,ab,a*b,...,a— 1 b] 

We easily see that 7Z is the set of states that can be reached from the origin. 
Lemma 3.1 The reachable subspace 72 is A-invariant, i.e. 

Ancn 

In particular, e M n C n for all t ^ FT . 

Proof Since, by the Cay ley- Hamilton theorem, A n is a linear combination 
of A* for . . ,n-l it follows that 

An= [ab,a s b,...,a”b\ c im [B'AB,...^*-* b] =n. 

Moreover, by induction we get A j K C 72, which implies that 

e At n = Y J t —A i ncn □ 

j=o 3- 

To further clarify the picture we note that if the state of the system is in n, 
at some instant, it is impossible to steer the state out of n. Neither is im- 
possible to enter 72 from an initial state not in 72. Particularly we have that 
if z 0 , e 72 then the state transfer can occur in just any time t. The points 
that can be reached in a time t from a given xo establish the plane 

n(x 0 , f) = e J ^ t xq -f- 72. 
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4 Stability 

A very essential problem when designing a control system is how to avoid 
instability, i.e. that the output increases without limit. The following sec- 
tion is an abridgement of chapter four in “An Introduction to Mathematical 
Systems Theory” by A. Lindquist and J. Sand [Lindquist/Sand]. All theory 
dealing with the alternative approach, the Lyapunov equation, is omitted. 

Intuitively an input-output system is stable if a bounded input produces 
a bounded output or if the output tends to zero, or at least remains bounded, 
when the input is zero. 

For nonlinear systems, stability in this sense is typically dependent on the 
initial conditions and the specific input applied. Hence, in general, stability 
is not the property of a system, but rather the property of a solution. 

This chapter deals only with the stability of time-invariant linear systems, 
a subject which is drastically simplified by the fact that the complete set of 
solutions of the system x = Ax can be displayed explicitly by means of the 
Jordan form. As a consequence, it is enough to check the eigenvalues of A 
in order to determine whether a bounded input produces a bounded output, 
and thus it will be meaningful to talk about stable systems. 

4.1 Stability of Continuous-Time Systems 

We want a bounded input to give a bounded output, which is sometimes 
abbreviated as B/50-stability. 

Definition 4.1 The system 

( x(t ) = A(f)x(t) + B(t)u(t) 

\ y(i) = C(t)x(t) 

is input-output stable if there is a k such that 

i%7i < 6 [< 0 , 00 ) h IW1) " 1€| '"’ 0O) 


for every t 0 . 
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Example 4.1 Consider the time-invariant case, where ( A,B,C ) are constant 
matrices. Then 

y(t) = f' Ce A(t - 3) Bu{s)ds. 

Jt 0 

Defining G(t ) = Ce At B, 

||y(OII ^ / 1 HG(< - 5)|| ||u(a)||<fa ( since ||u(t)|| < 1) 

Jt 0 

< r \\G(a)\\da 

Jt 0 

<I|C||||B|I /"" lle^llJT, 

Jt o 

i.e., a sufficient condition for input-output stability is that the integral 
fo° ll e ' 4t ||^ 1<S convergent. □ 

4.2 Stability matrices 


Let us study the homogeneous system: 

Equation 4.1 i = Ax ; x (0) = x 0 . 

Definition 4.2 77ie system (4-1) is stable if the solution is bounded on the 
inteTrval [0,oo) for all initial values xq and asymptotically stable if x(t) — ► 0 
when t — ► oo for all Xq. 

Theorem 4.1 (1) The system (4-1) is asymptotically stable if and only if 
the real parts of all the eigenvalues of A are less than zero, i.e. the eigenvalues 
are all located in the open left half plane. 

(2) The system (4-1) is unstable if A has at least one eigenvalue in the open 
right half plane. 

Proof In this proof we shall use a fundamental result from linear algebra, 
the Jordan decomposition theorem. This theorem guarantees the existence 
of a basis for 7 Z n in which the representation of the linear mapping A takes 
a particularly simple form. 



4 STABILITY 


12 


Transform the matrix A to Jordan form A = TJT 1 , where J is a block- 
diagonal matrix. 

J = diag(Ji , Jj,..., J r ) 
and each d v x d v block J v has the form 


' A„ 1 O' 
A„ 1 

**. 1 

. 0 A v _ 


A* being an eigenvalue of A. Thus, 


r e Jlt 


e At = T 


oJit 



so it remains to analyze each e Jvt . But J v has the form 


J v = A V I -f- S v 


where S v is a shift matrix 


S v 


0 1 O' 

0 1 

o ■*. 


0 


1 

0 


of dimension d v x d v , having the property that S' = 0 for i> d v . Conse- 
quently, 


— e^ v ^e^ vi = e^ vi 


' t * , t **- 1 

' + ts + T\ s +•••+ (£37)T 




and therefore, setting cr v = Re A„ and = 7mA,, 
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Equation 4.2 e At = e 17 ' 1 * P v (t)(cosu; v t + sinuj v t), 

where P v (t) is a matrix- valued polynomial of dimension d v — 1 in t. From 
this expression it follows that (1) e At x 0 -> 0 for all x 0 if and only if 
= Rc\ v < 0 for all v and that (2) e At xo — ► oo for at least one xq if some 
a v > 0. □ 

Lemma 4.1 The system in equation 4.1 is stable if and only if all eigenvalues 
of A are located in the closed left half plane and any eigenvalues on the 
imaginary axis correspond to one dimensional Jordan blocks. 

Proof By theorem 4.1 (1) we only need to worry about terms in (4-2) for 
which cr v = 0, i.e. e <7vt = 1 . These terms will remain bounded if and only if 
the degree of P v is zero, i.e. d v — 1 . 

Definition 4.3 A is a stability matrix if Re A(>1) < 0. 

Theorem 4.2 If A is a stability matrix then the time invariant system 

{ x = Ax 4- Bu 

y = Cx 

is input-output stable. 

Proof If all eigenvalues of A have negative real parts so that all a v in (4-2) 
are negative then 

roo 

/ \\e Ai \\dt <oo 
Jo 

and hence, in view of example 4.1 the system is input-output stable. □ 

The last theorem is very important for us because it deals with the kind of 
system we use when modeling an aircraft. 
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5 The Systems 

The test was accomplished with three systems of different kinds. All systems 
use a single input and produce a single output, a so called 5/50-system. 
As we will see later all systems have the necessary property of complete 
reachability, as was discussed in chapter 3. 

That a bounded input gives a bounded output is sometimes abbreviated 
as 5/50-stability. This highly desirable property for a system was discussed 
in chapter 4 and will be further examined for each specific case. 

5.1 Transfer Functions 

This subject is discussed in Etkin’s book “Dynamics of Atmospheric Flight” 
[Etkin, page 50-51]. He writes 

System analysis frequently reduces to the calculation of system 
outputs for given inputs. A convenient and powerful tool in such 
analysis is the transfer function, a function G(s) of the Laplace 
transform variable s [Complex valued], that relates input u(t) and 
output y(t) as follows, 

G(s) = y(s)/u{s) 

where (") denotes the Laplace transform. So long as u(t) and 
y(t) are Laplace transformable the transfer function defined above 
exists. However, it will in general be a function of the initial 
values of y and its derivatives, and moreover, for nonlinear and 
time varying systems, of the particular input u(t) as well. Such 
a transfer function is of relatively little use. We can however 
obtain a unique function G(s) if (I) the system is linear and time 
invariant, and (II) it is initially quiescent, i.e. at rest at the origin 
in state space with no inputs. 

He continues, 

When u(t) and y(t) are zero for t < 0, the Laplace and Fourier 
transforms are simply related, i.e. u(iu) = t/(u>). It follows that 
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Sometimes it is G(iu>) that is called the transfer function. 

When examining different transfer functions in a Bode diagram it shows 
that there axe systems with the same absolute value curve but with different 
phase curves. Of all systems with the same absolute value curve there is 
one with less negative phase advance, it is called a minimum phase system 
[Glad/Ljung, page 109]. 

I give the following theorem without a proof. 


Theorem 5.1 A theorem, with a rational transfer function is in minimum 
phase if and only if it has neither poles nor zeros in the open right half plane. 

The others are called non minimum phase systems. This distinction is very 
important because we know from one dimensional control theory that a sys- 
tem with zeros in the numerator will start off in the opposite direction. This 
bad quality can make the system difficult to control. 

A A-value less or equal to zero are assumed in the following calculations. 


5.2 System 1 



gives the system dynamics y = Ay + u 
The reachability matrix 


has full rank for all lambda and the system is therefore completely reachable. 
System l’s transfer function 

without neither poles nor zeros in the open right half plane indicates that it 
is a minimum phase system and should therefore be easy to control. 
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The Jordan transform of the matrix A is P~ 1 JP where 



p_ A/(A — 1) 1/(1— A) 
0 1 


Because all eigenvalues of A are located in the closed left half plane and the 
eigenvalue on the imaginary axis correspond to an one dimensional Jordan 
block we know that the system is stable. Owing to this quality we are guar- 
anteed that when a fortuitous in signal ultimately equals zero, the solution 
to the system x = Ax is bounded on the interval [0,oo). This of course im- 
plicates that also the output is bounded. Referring to previous theory the 
eigenvalue on the imaginary axis prevents input-output stability. 


5.3 System 2 


‘oioo] ro- 

• ° A1 i 0 , e f , 

0001 ^o" 7 y = [ 1 o 0 0 ] X X = 

. 0 0 0 A2 J 1 J 

gives the system dynamics y = A 1 y + u + ew ii = \2u + w. 

The reachability matrix 

' 0 e eAl cAl 2 + 1 
P _ e eAl eAl 2 + 1 cA 3 + Al + A2 
0 1 A2 A2 2 

. 1 A2 A2 2 A2 3 



has full rank for all values on A and t and the system is therefore completely 
reachable. 

System 2’s transfer function 
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This implies for a negative e that we have a zero in the open right half plane. 
As A 2 always is below or equal to zero the poles for a positive e are in the 
left half plane. So the system should be easy to control for a positive e and 
probably more difficult for a negative value on the variable. 

Taking the Jordan transform of A gives that J equals 

' A2 0 0 0 ‘ 

0 A1 0 0 
0 0 0 1 ’ 

.0 000 . 

Carrying through the same discussion as for system 1 we see that system 2 
has the same properties, stable but not guaranteed input-output stable. 

5.4 System 3 


' 0 1 0 0 0 1 [o' 

0 A1 1 0 0 e 

i = 0 0 01 0 x + 0 u; y = [ 1 0 OOOlarx 

0 0 0 0 1 0 

.0 0 0 0 A2 J 1 

gives the system dynamics y = \ly + u + ew u= X 2ii + w 
The reachability matrix 

'0 e eAl eAl 2 eAl 3 + l 

e eAl eAl 2 eAl 3 -f 1 eA 4 + Al + A2 

r = 0 0 1 A2 A2 2 

0 1 A2 A2 2 A2 3 

1 A2 A2 2 A2 3 A2 4 



has always full rank and the system is therefore completely reachable. 
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System 3’s transfer function 

es 3 — e\2s 2 + 1 TU-\ 

- (A 7 + A 2)s + XI • A 2) 

can be examined by Routh’s algorithm. The numerator es 3 — eX2s s + 1 
gives the following tableau. 

‘ e 0 ‘ 

— eA2 1 
1/A2 0 
.1 0 . 

As A2 always is below, or equal to, zero we get for a positive t that the left 
side coefficients t > 0 -eA2 > 0 1/A2 <0 1 > 0 change sign two times. 

This indicates that the system has two zeros in the open right half plane for 
a positive e. The same calculations for a negative e gives that the system has 
one zero in the open right half plane. 

The solution to the denominator 

s 3 {s 2 - (A 1 + A 2)s + \l-\2) = 0 

gives that for all negative values on XI and A 2 we have three zeros on the 
imaginary axis and two zeros in the open left half plane. If either XI or A 2 
equals zero we get four zeros on the imaginary axis and one in the open left 
half plane. When all eigenvalues equal zero we get of course all zeros on the 
imaginary axis. 

All this together gives that system 3 never will be a minimum phase 
system and will therefore be more difficult to control. 

Taking the Jordan transform of A gives that J equals 

' A2 0 0 0 0 ' 

0 A1 0 0 0 

0 0 0 1 0 

0 0 0 0 1 

0 0 0 0 0 

and this implies that system 3 is stable. The always present eigenvalues that 
equal zero prevent the system to be guaranteed input-output stable. 
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6 Derivations 

The fundamental idea of the test was to simulate an aircraft that is directed 
by the flight control to follow a certain path. The given trajectory can be 
seen as a set of points that shall be passed at a certain time. By way of 
introduction our intention was to determine how exact a given path in three 
dimensions could be tracked with maintained comfort for the passengers. 
Priority one was to minimize the acceleration and thereby the stress to the 
individuals. 

To start with, the test was accomplished in one dimension. This sim- 
plify the calculations radically and is a common approach to such experi- 
ments. Given the set of points {y®, yi , . . . , y B } and the corresponding time 
{to, we would like to perceive the control laws {u 0 , u t , . . . , u n } 

that take the system through the points in such a pleasant way as possible. 
Consider the control u k that takes the system from state vector x k to 

x k+l • 



Because t 6 [t*, t k+t \ the state of the system will be 

x(t) = e A ^-‘^x k + f* e A ^~ a ^Bu k (s)ds 

Ji k 

and as the state of the system is xt +1 at time t k+1 we receive the condition, 

Equation 6.1 

x k+1 = e M*k+i-tk) Xk + f ik+I e Mh+i Bu k (s)ds. 

Jt k 


The solution u k to equation (6.1) that minimizes the energy of the control 
signal is then given by, see chapter 3. 

Equation 6.2 


u k {t) = B T e~ ATt e- A ‘BB T e~ AT ‘ds^ ‘ ( 


e~ Atk + J x k+I - e~ Att 


x kl 
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That would be very convenient if the integral in equation (6.2) could be 
simplified in any way. 

We can see our flight path as the aircraft is flown through a large number 
of points by an autopilot. With a specific time interval the plane receives a 
correction signal that makes the vehicle track the path with high accuracy. 
The time interval t k+1 — t k are constant and determined by the frequency 
the automatic pilot works with. 

Assumption 6.1 Let t k +i — t k = h. 

Assumption (6.1) can be used to simplify the integral in equation (6.2). 
[ tk+1 e~ As BB t e~ AT ‘ ds = {r = s - t k } = 

e~ Atk [ H e~ M B B T e~ ATT dr e~ ATtk 

£ 

matrix constant 

Definition 6.1 

M ^ f h e~ Ar BB T e~ ATr dr 

Jo 

The equation (6.2) can be rewritten as 

Equation 6.3 

u k {t) = B T e- AT ( , - lk) M- , (e- Ah x k+I -x k ) 


The control would be specified completely by (6.3) if the whole state vector 
at each interpolation point was known. As only the points (yo, Vi , . • • , y») are 
known we have to apply some kind of conditions on the equation to obtain 
a solution. 

The control u is the actual control that the pilot or the auto-pilot achieve. 
A very natural choice is therefore to require that the control u is continuous. 

Assumption 6.2 


Uk(t k +1 ) = U k + i ( t k+1 ) 
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By this we acquire (n-1) conditions and allow us to write 

Uk(tk+l) = U k +l{tk+l) 


B T e- ATk M-‘(e- Ak x k+l - x k ) = B T M~ l {e~ Ah x k+t - x i+1 ). 
This equation can be simplified by 
Definition 6.2 

Z = M~ l e~ Ah 

W A e- ATh M~ 1 e~ Ah + M' 1 


and finally we obtain the modified expression 


B T (Z T x k - Wxt+j + Zx k+S ) = 0 k = 0, - 2. 


Written in block diagonal form it becomes, 

Equation 6.4 


' Z T -w z 
0 Z T -w 


0 0 0 
0 0 0 


0 0 0 
0 0 0 

-W Z 0 
Z T -W Z 


Xo 

Xi 

x 2 

X n - 2 
Xn— 1 
X n . 


As our three systems are very different the calculations will differ from here 
and all further computations have to be treated separately. 
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6.1 System 1 


This is the original system that was first implemented in Matlab. Each state 
vector Xi consists of two parts, a known coordinate y* and an unknown ve- 
locity yic. By partitioning the matrixes in definition 6.2 as 


z = 

*11 

*12 

II 

E-i 

Nj 

*u 


, w = 

W U 

W l2 


*2! 

*22 ^ 


*12 

222 



W 2 2 m 


and using the notations given in the following definition, the unknowns can 
be kept on the left hand side and the given position coordinates can be moved 
to the right hand side. 

Definition 6.3 


W, B = B 


Z* = B‘ 


Z,f = B T 


Wf = B' 


Z„=B‘ 


Z5 = B 1 


W21 

W 22 

*12 

*22 

*21 
Z 22 

W U 

W 2 1 

Z\l 
z 21 

*11 

*12 


= [%] 

= M 
= M 

= [wsi] 

= M 
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We get the system 


Uf 

-w t B 


0 

0 

0 


2/o 

Vi 

0 

Z [ f 

-W B 

0 

0 

0 


2/2 

0 

0 

0 

... -W, B 

Zl 

0 


lln—2 

0 

0 

0 

... Z B 

-W B 

Zfl. 


2/n-l 
- 2/n . 


’ -Z B W B -ZB ... 0 o 0 


yo 

2/i 

0 -Z B W B ... 0 0 0 


2/2 

o o 0 ... W B -Z B 0 


2/n— 2 

. 0 0 0 ... -z B t W B -Z B 


2/n— 1 

[ 

- 2/n . 


The right hand side consists of known parameters and is therefore a constant 
vector. Our system has (n+1) unknowns but only (n-1) equations so we need 
two more constraints. 

After reading Per Enquist s paper “Control Theory and Splines, applied 
to Signature Storage” [Enquist] I decided to use the natural boundary con- 
ditions, y 0 = 0 and y n = 0. This can be seen as a very real behavior for a 
vehicle and has also given the best results in former experiments. Enquist 
writes “This will let the initial direction and constant velocity of the system 
be decided so that the control energy is minimized” [Enquist, page 16-17]. 
The system dynamics equation y — \y -j- u gives 

Aj/o + u 0 = 0 where u 0 = B T M~ 1 (e _i4A x/ - = B T Zx, - B T M~ 1 x 0 . 

Definition 6.4 


U B = B t 


m n m 12 ‘ 
m -n m 22 


TjB 

U lu 
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We get 

(A - Ug) y 0 + z»y t = U B y 0 - Z* Vl 

The system dynamics equation together with earlier definitions give 
Ay™ + Un-i Un) = 0 where u„_; ( t n ) — B t e~ A * * M~‘ {e~ Ah x n — x„_) ) = 

B T e~ ATh Zx n - B T e- ATk M- , x n „ l = B r Wx n - B T M~ 1 x n - B T Z T x n . I 
We get 

-Zgy n -t + (A + W, B - U B ) y n = Z*y n - t + (U B - W r B ) y n 

Add these two equations to our considered system and the number of un- 
known parameters equals the amount of equations. Thus is the problem 
solvable and the Matlab program that uses Gaussian elimination is displayed 
in mprl21der0.m. 


6.2 System 2 


By this system a new approach was introduced for the convenience of the 
passengers. Instead of direct using the performed control signals we use its 
derivatives to control the aircraft. The formula it — X2u + w gives the con- 
nection between the control u induced by the pilot and the artificial control 
w that actually flies the plane. 

Each state vector £* consists of the known coordinate y* and the unknown 
parameters, velocity y*, control signal u * and its first derivative ti*. As we 
have (n-f 1) state vectors and each state vector consists of three unknowns 
it becomes a total of 3(n+l) unknown variables. The constraint that we 
require the control signal u to be continuous gives only (n-1) conditions. If 
the restrictions are introduced that also u and u have to be continuous, we 
get further 2(n-l) conditions. 

Assumption 6.3 


£*(£*+; ) = ujfc+J (**+i ) 
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Applying this to equation (6.3) becomes for the first condition 

B t A t ( Z T xt — Wxk+i 4- Zxt+g'j = 0 k = 0, 1 , . . . ,n — 2 
and for the second condition 

B t A t A t (z T xt - Wx k+l + Zx i+2 ) = 0 k = 2. 

By partitioning the matrices in definition 6.2 as 


z ll 

z \i 

213 

Z 14 



' ^11 

z 2l 

Z 31 

z 4l 

z 2l 

z 22 

223 

Z24 


T _ 

Z 12 

^22 

z 32 

z 42 

Z 31 

232 

233 

z 34 



Z 13 

z 23 

z 33 

z 43 

. z 4l 

242 

243 

z 44 _ 



. Z 14 

^24 

z 34 

z 44 . 




’ ^11 

w 12 

™13 

W U ' 






w = 

^21 

w 22 

™23 

W 2 4 







^31 

W22 

™33 

™34 







. ^41 

w 42 

^43 

w 44 m 





and using the notations given in the following definition, the unknowns can be 
kept on the left hand side and the given position coordinates can be moved 
to the right hand side. The matrix notation . symbolizes a whole row or 
column. 

Definition 6.5 

Continuous control signal, u: 

Wi = B t [w.2 w.3 u ;.^] = [ewgg + w^g twgg + W43 ^24 + W44 ] 

Z>w — B t [z, 2 Z'S z 4 ] = [ez 22 + Zj2 tzgs + Z43 ezgj + Z44] 

= B T [zg . Z3 . z^,] = [ tzgg + zg4 tzsg + Z34 eZjg + Z44] 

W r B = B T [w.j] = [ew sl + W41 ] 

= B t [z a \ = [ezgj + zjj\ 

Z A = B T [zi] = [ezjg + Z14] 
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Continuous first derivative of control signal \ u: 

W B — B t A t [W'£ W'S w.4] — [twtz 4 eXlwt* 4 w st 4 X2W42 
twis 4 eXlw S s + u) 33 4 \2w43 twn 4 tXlv)24 4 W34 + A 2 w u ] 


Z B = B T A T [z,2 Z.s Z'j] = [tZi2 4 eXlZ2S 4 Z32 4 X2z+ 2 
£ZlS 4 tXlZ 23 + Z33 + A Szjs tZj4 4 tXlZ24 4 Z34 + A 2Z44] 

Z B = B t A t [z2. Z3 . Z4] — [ez2i 4 tXlz2t 4 Z23 + X 2 z %4 
tzst 4 tXlzs2 + Z33 + A 2zs 4 tZ4t 4 eX IZ42 4 * Z43 4 X2Z44] 


W B = B t A t [wj] = [ewu 4- tXlw 2 i 4- W 31 4 X 2 w 4I ] 
Zfl = B t A t [z a \ = [tzu 4- tXlz 2 i 4- Z 31 4* X 2 Z 41 } 
Z B = B t A T [ zt ] = [tzu 4 eXlzi2 4- Z 13 4- X2z^] 

Continuous second derivative of control signal , ii: 

W B = B t A t A t [w.2 w .3 w.4] = 

[cX 1 Wj2 4 " ( cXl 2 4 * ^ ) vj, 22 4 A 2w3% 4 A 2 W42 
tXlwi3 4 (eXl 2 4 l')w 2 s 4 A 2W33 4 A 2 W43 
cX I1V14 4 (eA 1 2 4 1 ) to 24 4 X2W34 4 A 2 W44 ] 

Z B = B T A t A t [z.2 Z'S z 4 J = 

[eXlzi 2 4 (cXl 2 4 1 )z 2 g 4 A 2 Z 32 4 A 2* Z 42 
eXlzis 4 (cXl 2 4 1 )z23 4 A 2zss 4 A 2 s Z 43 
eX IZ 14 4 (cA I s 4 1 )z 24 4 X 2 Z 34 4 A 2 s Z 44 ] 
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Zfi =B T A T A r {z t , z s . z 4 ] = 
[eXlzgi + (tXl 2 + l)zg 2 + \2zgg 4* X2 2 zg 4 
tXlzgi 4* (eXl 2 4- 1 )zs 2 4* X2z 33 4~ A 2 s z 34 
tXlz 4l 4" (cA 1 4" 1 )z 4 z 4" X2z 43 + A 2 s z 44 ] 


Wfi = B t A t A t [w . i ] = [eXlwu +(eA I s + l)w zl + X2w 3t +X2*w 4 , } 

Zfi = B t A t A t [zj] = [eXlzu + (eA I s + l)z sl 4- A 2z si 4- A 2 s z 41 ] 

Zfi = B^ A^ [z i .] = \tXlzn 4~ (cA 1 2 4" 1 )z 4 s 4" A 2zi 3 A X2 2 zj 4 ] 

We get the following systems, written in block diagonal form. 

Each state vector is divided into two parts, a known portion x 2 which 
contains the given position y* and an unknown portion x% that contains the 
parameters y*, u* and li*. All right sides consist of known variables and are 
therefore constant vectors. 

Continuous control signal u: 









1 

'zfi - 

-wfi 

Zfi 

0 

0 

0 



0 

Zfi 

-wfi 

0 

0 

0 


*2 

0 

0 

0 

... -wfi 

Zfi 

0 


X n-2 

0 

0 

0 

... Zfi 

-wfi 

1 

N 


X n— 1 








< J 







Xq 

■ -Zfi, 

wfi 

-Zfi u 

0 

0 

0 



x[ 

0 

-Zfi 

wfi 

... 0 

0 

0 


x\ 

0 

0 

0 

... wfi 

-Zfi 

0 


T P 

x n- 2 

0 

0 

0 

... -zfi 

wfi 

-Zfi, 



r p 

x n- 1 
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Continuous first derivative of control signal, u: 



Continuous second derivative of control signal, it: 
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Having totally 3(n+l) unknowns but only 3(n-l) constraints we chose to 
enter differential approximations for the first and last state vector, this will 
decrease the number of unknown parameters by six and thus make the prob- 
lem solvable. Remember that the time interval tt +I — tt is constant and 
represented below as h. 

The needed velocities are approximated as: 


Vo = 


yi - yo 


y» = 


y n - y n -i 


The needed control signals are approximated as: 


Vi = 


ye ~ Vi 


yn-t 


n 0 = 


Vn—i y »— S 

h 

yo - 1 )i 


u» = 


yn-t - y n 


The needed first derivative of the control signals are approximated as: 

ye ~ yt 


ye 


J/n-2 = 


Uj = 


Vn — 2 J /n — J 

h 

Vl ~ Vt 


u n -i - 
Uo 
U n = 


Vn — 2 Vn — i 

h 

u 0 — Ut 




The Matlab program that solves the task for system 2 using Gaussian elim- 
ination is displayed in mprl41knovel.m. 
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6.3 System 3 


This system uses the same approach for the convenience of the passengers 
as system 2 does. The only difference is that we now also use the third 
derivative of the control signal u to actually fly the plane. The formula 
ii= X2u + w gives the connection between the control u induced by the pilot 
and the artificial control w that actually flies the plane. 

Each state vector x k consists of the known coordinate y k and the unknown 
parameters, velocity y*, control signal u k , its first derivative tit and its second 
derivative rzjt . As we have (n+1) state vectors and each state vector consists 
of four unknowns it becomes a total of 4(n+l) unknown variables. The con- 
straint that we require the control signal u to be continuous gives only (n-1) 
conditions. If the restrictions are introduced that also ti, it and ii have to be 
continuous, we get further 3(n-l) conditions. 

Assumption 6.4 

Uk(tk+1 ) = lii-M {tk+1 ) 

Uk(tk+1 ) = Ui+/(*i+/) 

(tk + l) =«i+/ (tk+t) 

Applying this to equation (6.3) becomes for the first condition 

B t A t {Z T x k — Wxk+i +Zxk +S ) = 0 k = 0, 2, 

for the second condition 

B t A t A t [Z T Xk — Wxk+i +Zx k +s) = 0 k = 0,l,...,n- 2 

and for the third condition 

B t A t A t A t (Z T Xk - Wx k+i +Zx k+S ) = 0 k = 0,l,...,n- 2. 

By partitioning the matrices in definition 6.2 as 


z ll 

Z 12 

Z 13 

z 14 

Z V> 


z ll 

Z 21 

Z31 

Z41 

Z 51 

z 2l 

z 22 

z 23 

z 24 

z 25 


Z 12 

z 22 

232 

z 42 

z 52 

z 3l 

z 32 

z 33 

z 34 

z 35 

Z T = 

z 13 

z 23 

233 

z 43 

z 53 

Z 41 

z 42 

z 43 

z 44 

z 45 


z 14 

z 24 

2 34 

Z44 

z 54 

z 5\ 

z 52 

z 53 

Z S4 

z 55 


. Zl5 

z 25 

2 35 

z 45. 

z 55 
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w 11 

w 12 

W 13 

W 14 

™15 

W 2 \ 

w 22 

W 23 

W 24 

W 2 5 

mi 

™32 

™33 

^34 

ms 

W 4 1 

W 42 

W 43 

^44 

ms 

mi 

1^52 

W53 

^54 

ms 


and using the notations given in the following definition, the unknowns can be 
kept on the left hand side and the given position coordinates can be moved to 
the right hand side. The matrix notation . symbolizes a whole row or column. 


Definition 6.6 

Continuous control signal, u: 

W, B = B t [w,z w.s w _ 4 w. 5 ] = 

[ew S s 4 w S 2 ewzs 4 w 5S ew 24 4 w 54 ew S5 -|- w 5S ] 

~ & l z -2 z s z .j, Z's\ = [ezs 2 4- z 5S tzzs 4 z$s czg 4 -f- Zg 4 ez 2 $ + z$ s \ 

^ [ Zs - Zs ■ Z 4- Z s\ = [t z S2 4 " Zgs CZgg 4 Zss CZ 4 g 4 " Z 4 $ CZ$2 4 Zjj] 

W B = B T [W' I } = [ew st 4- W 51 ] 

= B T [ z .i\ = [ezgj 4- zgt\ 

Z* = B T [ z i] — [czjg 4- -J/j] 

Continuous first derivative of control signal, u: 

Wi — BA [W'2 w.$ W'4 iv. 5] = " 1 “ cA I1V22 + W42 + \2ws2 

tw is 4- t\lw23 4- w 4 3 4- A 2wss tw 44 4 - eXlw S4 4- w 44 4- X2w 44 ] 


z u = B t A t [z.2 z.s z . 4 z.s] = [ez ts 4- tXlz ss 4- z 4S 4 \2z st 
tz is 4- tXlzgs 4- z 4S 4- X2z ss cz, 4 4 e\lz S4 4 z 44 4 A 2z 54 ] 


Z u - B t A t [z 2 . z s . z 4 . z 5 ] = [ez SI 4 eXlz 2S 4 z S4 + X2z S5 
ez 3i 4- eX lz ss 4 z S4 4 A 2z 3S ez 41 4 tXlz 4t 4 z 44 4 A 2z 45 \ 



6 DERIVATIONS 


32 


W? = B T A T [w,,\ = [ew u +e\lwzi + w 41 + X2w 51 ] 

= B T A t [z_i\ = [ez^ + eXlzgi + z 4l + X 2 zs t ] 

Z r i — B A t [zj] = [ezjj + cXlzjg + z^ + X 2 zjj] 

Continuous second derivative of control signal, ii: 

Wl B = B T A T A T [W'2 W '3 W'j W' 5 ] = 

[e\lwi2 + t\l 2 w 2 2 + W32 4- X2w 4S + X2*w S 2 
eXlwts 4" 4" -|- X2 2 w 33 

eXlw 14 + eXl 2 w 24 + w S4 + X2w 44 + X2 2 w 54 ] 

Z B = B r z4 r /l r [^ £ zj z 5 ] = 

[cA iz/g + ^A 1 2 Z22 4" %S2 4* X2z 4 2 4" X2 2 z$2 

eXizis eX 1 Z23 4- z$s 4- A 2z 4 3 4 - A 2 2 z$s 

tXlzj 4 + 6 A 1 2 Z 2 4 4" Zs 4 4- A 2z 44 4* A 2 2 z§ 4 ] 

Z* = B t A t A t [z2. z s . z 4 . z&\ — 

\tXlz21 4" eXl Z22 4“ Z23 4" X2 z 2 4 A~ X2 2 Z25 
eXlz 31 + eXl 2 Z32 + Z33 + X2z S4 4 - A 2 2 z S5 
eXlz 41 + eX l 2 z 42 + z 43 + A Sz 44 4 - A 2 2 z 45 ) 

= B A A [w^] = [eXlwjj + tXl 2 W21 + W31 4“ X2w 4 i 4* X2 2 w$i\ 
Zyu — B A A \z,i\ = [eXlzu 4- tXl 2 Z21 4" zsi 4* A 2 z 4 i 4- X2 2 z$i\ 

Zri = B A A [z/.] = [eXlzjj 4- eX l 2 Z12 4~ Z13 4- A 2z 44 4* A 2 2 Zt$\ 
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Continuous third derivative of control signal, u: 

W i — B T A t A t A t [W'2 w.s w.4 w.s) = 

[eXl W 12 + (eXl 3 + l)u )22 + X 2 w $2 4~ X 2 2 w ^2 + X2 S W 52 
tXl Wis 4 ~ (cA 1 3 + 1 )w 2 s 4 ~ A 2wss 4~ A 2 2 + X2 3 W 53 

eXl Wfj “f* (cA 1 4~ 1)^24 X2w$4 -f- A 2 W 44 A X2^ 

zf u = B t A t A t A t [z.s z.s Z . 4 z.s] = 

[tXl Z 12 4* (eXl 4* 1 )z 22 + A 2 z $2 4“ A 2 2 z ^2 4" X2^ z $2 
eXl Zis + (eXl 3 + 1 )^ 2 s 4" A 2z$$ 4 * X2 2 z±$ 4 “ X 2 s z$$ 
eXl zt 4 4~ (cA 1 4" 1^24 4- X 2 z 34 4- X2 8 z^ 4~ A 2 s z$jf\ 

zf/ = B T A t A t A t [z s . z s . z 4 . z 5 ] = 

[eXl 8 Z 21 4~ (cA l 3 4- 1 )%22 4" X2zbs 4- A 2 8 Z 24 4* X2 S Z 25 
eXl zsi 4~ ( tXl + I}z 32 4~ A 2z$$ 4* A 2 2 Z 24 X2 S z$$ 
eXl 2 zjj 4- (eXl 3 4- 1 )z ^ 2 4- A 2 z±$ 4- X 2 2 z^ 4- X2 S z±$] 

W B r = B t A t A t A t [w. i ] = 

[eXl Wn + (eAi 4“ 1 ) w bi 4” A 2w$t 4“ A 2 2 W 41 4” X2 S ] 

zl=B T A T A T A T \z,] = 

[eXl 2 zjj 4- {eXl 3 4* l)z 2 i 4- A 2z$i 4- X2 2 z^i 4- A 2 s z$f] 
Zrl= B t A t A t A t [z 1 ] = 

[eXl 2 ztt + (eX I s 4- 1 )zi 2 4* A 2 zi$ 4- X2 2 zn 4- A 2 s zj 5 ] 
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We get the following systems, written in block diagonal form. 

Each state vector is divided into two parts, a known portion x£ which 
contains the given position yt and an unknown portion that contains the 
parameters yt, u*, ut and u*. All right sides consist of known variables and 
are therefore constant vectors. 

Continuous control signal u: 








r 

1 

7 b 

“U 

-W t B 

zg 

0 

0 

0 


x\ 

0 

zg 

-W B 

0 

0 

0 



0 

0 

0 

... -W B 


0 


<-2 

0 

0 

0 

... zg 

-W, B 

Zl. 


K-x 







L 

< J 







xl 

7 b 

L rl 

w B 

-Z r i 

0 

0 

0 

1 


r p 

X 1 

0 

-z?i 

W r B 

0 

0 

0 


x\ 

0 

0 

0 

... W B 


0 


T P 

X n-2 

0 

0 

0 

... -Z B 

wf 

-Z» 



T P 
x n- 1 







x p 
L x n 
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Continuous first derivative of control signal, li: 








r 

* v 0 1 

'zg - 

-Wg 

Zg ... 

0 

0 

0 


x\ 

0 

Zg 

-Wg ... 

0 

0 

0 


X% 

0 

0 

0 

-Wg 

zg 

0 


<-2 

0 

0 

0 

zg 

-wg 

ZfL. 


x n-l 








*: J 



-Z B ... 




X P Q 

’ -Zg 

wg 

0 

0 

0 



*? 

0 

-Zg 

Wg ... 

0 

0 

0 


x\ 

0 

0 

0 

Wg 

-ZfL 

0 


T P 

x n-2 

0 

0 

0 

-Zg 

wg 

-ZfL 

J 


T P 

X n- 1 







x p 

- x n 


Continuous second derivative of control signal, it: 

' zg -wg zg 

0 zg -wg 

0 0 0 

0 0 0 


' -zg wg -Zg u 

0 -zg Wg 

0 0 0 

0 0 0 


0 

0 

0 ' 


U/ 0 

x\ 

0 

0 

0 



-wg 

ZfL 

0 


X n- 2 

Zg 

-wg 

zg. 






L 

*: J 




x\ 

0 

0 

0 



x{ 

0 

0 

0 


x\ 

wg 

-Z,“, 

0 


r p 

x n-2 

-Zg 

wg 

7 b 

6 tu 

J 


T P 

X n- 1 
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Continuous third derivative of control signal, u: 


B 

...B 

■ ■■B 



. 


r t v i 

Z tl 

- w, 

-B 

Zlu 

... B 

0 

0 

0 


X? 

0 

Ztt 

- w, 

0 

0 

0 


x\ 

0 

0 

0 

... B 

... - w, 

a 

6lu 

0 


x n- 2 

0 

0 

0 

...B 

... Zu 

— B 

— w, 

...B 

Zlu . 


T v 
x n- 1 

X v 

- x n J 


— B 
- Z T l 

• •• B 

W r 

-B 

— Z ru 

0 

0 

0 


r r p 1 

0 

-B 

-Z w 

... B 

w r 

0 

0 

0 


^ 1 

0 

0 

0 

• B 

... w r 

—B 

Z ru 

0 


®n-2 

0 

0 

0 

...B 

... -Zw 

— B 

W r 

■•■B 

Zru . 


^-1 

J 


Having totally 4(n+l) unknowns but only 4(n-l) constraints we chose to 
enter differential approximations for the first and last state vector, this will 
decrease the number of unknown parameters by eight and thus make the 
problem solvable. Remember that the time interval t k+1 - t t is constant and 
represented below as h. 

The needed velocities are approximated as: 


yo = 


2/n = 


m - yo 

h 

Vn Vn — 1 


The needed control signals are approximated as: 


yn-I 


U 0 = 


Vn-t Vn— & 

h 

yo - yt 
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«n = 


Vn—l Vn 


The needed first derivative of the control signals is approximated as 

Vt ~ yi 


ys = 


Vn-S = 


u, = 


Vn — 2 2/n — 3 

h 

yi - its 


U n -i = 
U 0 = 
Wn = 


y n - 2 - j/ n _, 

h 

U 0 - Uj 

h 

U n -t - u n 


The needed second derivative of the control signals is approximated 

ys - ys 


ys = 


y n -s = 


u s = 


Vn — 3 Vn—4 

h 

ys - ys 


U n -2 = 

U t = 


Vn—S lln — 2 

h 

Ut - u t 


Un~l = 


U 0 = 


Un — 2 - Un-i 

h 

Uo “ Ut 


U n - 


Un — 1 “ U n 


as: 


The Matlab program that solves the task for system 3 using Gaussian elim- 
ination is displayed in mpr!51knovel.m. 
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7 The Test 

The test was effected by forcing the current system to run through points 
situated on the curve we wanted to track. This denotes that the trajectory 
has total freedom between the fixed coordinates as long as it passes through 
the test curve dots. How close the system followed the test curve was mea- 
sured at five additional points between each two fixed coordinates. All these 
extra measuring points, along the curve, were added by their absolute value. 
The sum of all these measurements is called total-error. 

Point-error shows how precisely the system tracks the fixed coordinates. 
During all the trials, this value always been zero, i.e. perfect tracking. The 
only fixed coordinate that the system does not run through is the end point. 
It is due to the lack of constraints there and its divergence is measured by 
End-point-error. 

7.1 Test curves 

Three different kinds of test curves with diverse characteristics were used. 
The two standard curves are one period of the sharply curving sine function 
and the soft curving function, the hyperbolic tangent. A discontinuous step 
function was also used in the test, (see below). 


On* dim. frq: mpr1216*f0.m lambda - -1 n - 10 



Figure 1: The step function tracked by system 1 with A=0. 
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One dim. traj: mpr121d«f0.m lambda - -1 n - 26 



Figure 2: The sine function tracked by system 1 with A=0. 


One dim. traj: mpr121dat0.m lambda - -1 n - 20 



Figure 3: The tangent hyperbolic function tracked by system 1 with A=0. 
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8 Results 

In this chapter we are going to look at the output from our systems. The 
following figures are exactly like those shown on the computer screen. 

At the top left of the graph the program used is exhibited. At the top right 
are the values of A 7 , 11, A 2, 12, and e, ep, shown together with the number 
of points, n, used to determine the test function. The scale of the y-axis cure 
indeterminable but can give a hint about the ratio between variables of the 
same kind. Which quantity the plot gives information about is displayed to 
the left of the axis. At the bottom is always the time axis, scaled in seconds, 
displayed jointly with the calculated errors, see chapter 7. 

At first the difference between system 1 and the other two systems will 
be shown. The two systems can be represented by system 3 since they have 
about the same behavior for this choice of parameters. Notice the sharper 
look of system 1 s control signal u and acceleration, the latter has less maxi- 
mal deviation in both graphs. System 1 can not be affected in the same way 


One dim. traj: mpr121def0.m lambda - -1 n - 26 



Figure 4: Sine tracked by system 1. 

as the other two systems. Due to this it is not so very interesting and will 
therefore from now on be omitted. 
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On* dim. traj: mprl 61 knovd.m II - -1 t2--t*p-.0n-26 


CSu 

ACC 


0 1 2 3 4 5 6 

Total error : 0.003882 Point error :0 Graph : 1 Timet 

Figure 5: Sine tracked by system 3. 

System 2’s behavior when tracking the soft curving function tangent hy- 
perbolic is shown below. The first graph shows the smooth behavior for the 
system when e = 0. The other two indicate a more uncontrolled fluctuation 
for an e ^ 0. The second and the third test are done with different c so the 
received maximum deviation for the acceleration felt by the passengers are 
of the same magnitude. The acceleration is at the bottom except when it is 
oscillating heavily as in the last two plots. 

Opposite the analysis made in chapter 5 it seems as though system 2 is 
not as easy to handle even for an e > 0 and its oscillations for e < 0 axe 
apparent. 

In two plots some parts of the curves are omitted. This is to prevent the 
large deviation of the acceleration at the endpoints to suppress other impor- 
tant information. However, this makes a correct error estimation impossible 
and the displayed values on the total error for these plots are wrong. The 
true value on the total error is 0.0997 for figure 7 and 0.1967 for figure 9. 
The last mentioned plot shows the influence of a changed value on A 2, It 
increases the magnitude and shifts the oscillating acceleration to an earlier 
time interval. For both systems it is true that XI affects the behavior much 
more than what A 2 does. 
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System 3’s conduct for XI = \2 = -1 and varying values on e, when tracking 
the tangent hyperbolic function, is shown below. 

According to the theoretical discussion in chapter 5, we stated that system 
3 has two zeros in the open right half plane for an e > 0 but only one for an 
e < 0. We see that the system can handle negative e better than positive, 
(see figure 11 and 12). 

The curve that shows the acceleration is mostly beneath the control signal 
u in all graphs. It is also the most oscillating signal in the two last plots. 

The tests using the tangent hyperbolic function are carried out with dif- 
ferent sets of A for each systems. It is therefore not so easy to compare 
the behavior for the actual systems. However, during experiments that are 
not presented in the report, it has been shown that system 3 is more easily 
disturbed for an e ^ 0 than system 2 is. 

Correct total error for figure 11 is 0.2435 and 0.3126 for figure 12. 



Figure 10: Tangent hyperbolic tracked by system 3. 
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System 3 s characteristics are further examined below when it is applied to 
the sine curve. Figure 5 shows the obtained control signal u and acceleration 
for Af = A 2 = -1 and e = 0. The first graph below displays the control signal 
w for the same system and parameters. This is to exhibit the calculated 
signals for a smooth case so we have something to compare with when it is 
getting rough. 



Figure 13: Sin tracked by system 3. 


We receive some other signals in the following figures when system 3 is run by 
an e ^ 0. Observe the twisted trajectory in figure 15 with an accompanying 
large value on the total error. Notice also the magnitude on the control signal 
w and its third derivative, shown in figures reffi:16 and 17. The system seems 
to be more sensitive for an t> 0 than for an € < 0, this is probably due to 
the circumstance that it has two zeros in the open right half plane in the 
first case but only one zero in the second case. The oscillating acceleration 
is shifted to a later time interval and has there a larger magnitude for a 
negative e. 
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Figure 14: Sine tracked by system 3. 


One dim. traj:mpc151krwvot.m II --1 12 - -1 ap - 0.002 n - 26 



Figure 15: Sine tracked by system 3 
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Applying the step function to system 3 gives that the very large control 
signals u can be reduced by an e ^ 0 to the cost of a larger total error. 




Figure 21: The step function tracked by system 3. 
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At last we will take a look at system 2 applied to the sine function with A 1 
= \2 = -1 and for a very particular choice of e. Figures 23 and 24 show 
an oscillating but quite normal behavior for this system. When examining 
figure 24 which is run with a slightly changed t it looks about as the other 
two until the scale on the y-axis is observed. The first two graphs use an t = 
0.004186 respectively e = 0.004188. The value of t for the last three figures 
is 0.00418702471143, which gave the largest oscillatory motions. It seems 
that we have found a set of parameters that brings system 2 in to resonance. 
When changing the value on t we might affect the transfer function so that 
the frequency of the actual in signal w is the peak frequency; see figure 
27. In such a case we will receive an increased amplification of the output 
signal. Notice that this phenomenon only appears when tracking the sine 
test function. 
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Figure 23: Sine tracked by system 2. 



Figure 24: Sine tracked by system 2. 
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8.1 Surprising Results 

Looking at figure 4 or 5 one might be surprised at the appearance of the 
control signal u and the acceleration. Kven if the sine function is curving it 
should not cause such peaks in the graphs. The reason for this is as follows. 
Consider the system 



From the given system we have that x, = x s and that x t = u. If we use 
splines and force the system to track the function f(t) it implies that the 
first element in the state vector, i/ , equals the function, i.e. y(t) = f(t). 
This denotes that Xg(t) = /(t) and that the control signal u(t) = f(t). The 
velocity in figure 28 should by our theory have something in common with 
the derivative of the curve f{t) = sin t. The derivative f(t) = cos t, so in the 
beginning of the first region when f(t) = -1, the graph is shifted upwards 
and scaled, the velocity should be zero. When f(t) becomes zero the velocity 
ought to reach its maximum and then decline. For the other half we have the 
reversed situation and should therefore have an inverted curve. This behavior 
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can be recognized in the first graph and a similar reasoning for the control 
signal u equals /(*) = - sin t gives the calculated control signal in figure 4. 
Here X ^ 0 so the graph is shifted to the right. So the splines do not only 
try to follow the specified trajectory they also approximates its derivatives. 
We can thus effect a perfect trajectory of the test curve but the prize we pay 
is huge values on the control signals and accompanying acceleration. The 
behavior described makes it impossible to realize a smooth aircraft control 
using splines. 

This very simplified heuristic description of the phenomenon will be com- 
pleted in a paper written at Texas Tech University, Lubbock, USA, by Horn 
Professor Clyde Martin, PhD, and Assistant Professor Zhimin Zhang, PhD. 



Figure 28: Sine tracked by system 1. 


6 
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9 Resume in Swedish 

Inledningsvis var varan avsikt att ta fram styrlagar for en flygplans modell 
sa att den flogs si behagligt for passagerarna som mojligt. Passagerar kom- 
forten var det allra mest vasentliga sa vi utvecklade tvi kontroll lagar som 
anvande derivatorna av den pilot inducerade styrningen «. Detta ger inte den 
energi snalaste insignalen men tar bort de varsta topparna hos styrsignalen 
och medfoljande acceleration. 

Programvaran som anvants inkluderar Matlab och Maple for berakningar 
och Latex som ordbehandlings program. Andra halften av rapporten bestir 
av Matlab program och som avslutas med en referenslista. 

Vi ansatte den vanliga endimensionella behandlings proceduren och im- 
plementerade systemen i Matlab. Ett huvud program, med tillhorande hjalp 
program, for varje kontroll lag. 

De givna systemen analyserades ur uppnibarhets och stabilitets synpunkt 
vilket resulterade i en bedomning att de var stabila men inte garanterade 
insignal-utsignal stabilitet. Se kapitel 5. 

En “spline” ar en kurva till ett n:te gradens polynom vilken ar forenat med 
liknande polynoms kurvor i respektive andpunkt. I varje forenings punkt har 
funktionerna sina n-1 forsta derivator gemensammma. Detta ger en kurva 
som av ogat tycks vara helt homogen men som i sjalva verket bestir av 
ett antal sammankopplade delar. Kapitel 6 behandlar “splines” och den 
anvandna kontroll teorin. 

Huvud resultatet var att vi inte kunde ta fram mjuka styrlagar eftersom 
“splinsen” inte bara forsoker approximera test kurvan utan aven tar hansyn 
till dess derivator. Genom systemet piverkas aven styrsignalen och vi erhiller 
omojligt stora styrsignaler och accelerationer. Noggranheten i foljningen ar 
alltid exemplarisk. Kapitel 8 behandlar rapportens huvudresultat. 
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10 Mat lab Programs 

function x m mprl21der0(spc_plot ,t ,n,cleargr, pointfcn, lambda) 

•/.*/. THIS PROGRAM CALCULATES CONTROLLAWS FOR A ONE 
DIMENSIONAL TRAJECTORY 7 . 7 . 

7.7. spc.plot DETERMINES WHICH GRAPH TO BE DISPLAYED, 

CHOSE AN INTEGER =<5 7 . 7 , 

7 . 7 . t IS THE TIME PERIOD FOR WHICH THE SYSTEM 
IS TO BE CONTROLLED 7 . 7 . 

7 . 7 . n ARE THE NUMBER OF POINTS AT THE SPECIFIED 
TRAJECTORY, CHOSE t/n>l/10 7 . 7 . 

7 . 7 . cleargr CLEARS THE CURRENT WINDOW, CHOSE 1 OR 0 7 . 7 , 

7 . 7 . pointfcn SPECIFIES THE TRAJECTORY 7 . 7 , 

7 . 7 . lambda AFFECTS THE INSTABILITY OF THE SYSTEM 7 . 7 , 

global A B h t n 

7 . 7 . THE SYSTEM 7 . 7 . 

A=[ 0 1 ; 

0 lambda] ; 

B= [ 0 ; 

1 ]; 

c=[ 1 0 ] ; 

7 . 7 . FUNCTION pointfcn DETERMINES THE SPECIFIED TRAJECTORY 7 . 7 . 

7 . 7 . NECESSARY FOR THE COMPOUND FUNCTION pointsinl2 7 , 7 , 

if pointf cn==’pointsinl2' ; 
t=5 . 2 ; 
n=26 ; 
end; 


R=feval (pointfcn) ; 
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7 . 7 . CALCULATION OF THE INTEGRAL FROM 0 TO h 7X 

m=48; 7 . 7 . NUMBER OF POINTS BETWEEN INTERPOLATIONS, 

CHOSE A MULTIPLE OF 6 IX, 
mp=m/6; •/,•/, DETERMINES THE PRECISION IN THE 

SPLINE APPROXIMATION 7.7, 

tol=le-08; '/X THE NUMERIC ERROR TOLERANCE IX, 

Mtau(: , l:2)=zeros(2) ; 
tau=0 ; 
for j=l:m 
oldtau=tau; 
tau=oldtau+h/m; 

Mtau( : , 2*j+l :2*j+2)= quad812mod( ’ integramd’ ,oldtau,tau,tol) 
+ Mtau(: ,2*j-l:2*j) ; 
end; 

M=Mtau( : ,2*m+l : 2*m+2) ; 

e_Ah=expm(-A*h) ; 

Minv=inv(M) ; 

ZZ=Minv*e_Ah; 

WW=e_Ah , *ZZ+Minv; 

WL=[WW(2,2)] ; '/.•/, PARTITIONING MATRICES IX, 

ZLU=[ZZ(2,2)]; 

ZLL= [ZZ(2 ,2)] ; 

WR= [WW (2,1)] ; 

ZRU=[ZZ(2,1)] ; 

ZRL= [ZZ(l ,2)] ; 

Ulu= [Minv(2,2>] ; 

Uru= [Minv(2, 1)] ; 

7.7. FORMING OF THE RIGHT HAND SIDE OF THE BLOCKDIAGONAL 
SYSTEM 7 . 7 . 

for i=2:n 

Omega(i , l)=-ZRL*R(l , i-l)+WR*R(l , i) -ZRU*R(l ,i+l) ; 
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end; 

Omegad , 1 ) = Uru*R(l,l) " ZRU*R(1,2); 
Omega(n+l,l)=ZRL*R(l,n) + (Uru-WR)*R(l,n+l) ; 

XX FORMING OF THE LEFT HAND SIDE OF THE BLOCKDIAGONAL 
SYSTEM XX 

for i=2:n 

DD(i , i-l)=ZLL; 

DD(i , i)=-WL; 

DD(i , i+l)=ZLU; 
end; 

DD(1 , l)=(lambda-Ulu) ; 

DD(1,2)=ZLU; 

DD(n+l ,n)=-ZLL; 

DD(n+l,n+l)=(lambda+WL-Ulu) ; 

DD=sparse(DD) ; XX SQUEEZING OUT ALL ZERO ELEMENTS 

FROM MATRIX DD XX 

XX GAUSSELIMINATION TO PRODUCE AN UPPER 
TRIANGULAR SYSTEM XX 

for i=l:n 

zd=DD(i+l ,i)/DD(i,i) ; 

DD(i+l,i)=DD(i+l,i)-zd*DD(i,i); 
DD(i+l,i+l)=DD(i+l,i+l)-zd*DD(i,i+l) ; 

Omega (i+1 , l)=Omega(i+l , 1) -zd*Omega ( i , 1 ) ; 
end; 

XX BACKSUBSTITUTION TO SOLVE FOR THE XVEL XX 

xvel ( 1 , n+ 1 ) =Omega (n+ 1 , 1 ) /DD ( i+ 1 , i+1 ) ; 
for i=n:-l:l 

xvel(l ,i)=COmega(i > l)-DD(i,i+l)*xvel(l,i+l))/DD(i,i) ; 

end; 
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MAKING OF THE STATE VECTOR ’/.*/. 
for i=0:n 

x( : ,i+l)=[[R(l , i+1)] 

[xvel(l , i+1)]] ; 

end; 

if cleargr 
elf ; 

hold on; 
grid on; 
end; 


•/.•/. PLOTTING OF THE CALC/SPEC TRAJECTORY, VELOCITY, 
CONTROLSIGNAL AND ACCELERATION */J, 

plotadm=0; 
while spc_plot 
if plotadm 
if spc_plot<6 
figure; 
hold on; 
grid on; 

title( ['One dim. traj : mprl21der0.m lambda = ', 
num2str(lambda) , ' n = ' ,num2str(n)] ) 
xlabel( ['Total error : ' ,num2str(Total_error) , ' 
Pointerror : ' ,num2str(Point_error) , ’ Graph : 

' ,num2str(spc_plot) , ' Timet']) 
for k=0:n 

plot(k*h,x(l ,k+l) , 'o') 
end; 
end; 
end; 

breakadm=0 ; 

f adm=0 ; •/,*/, NORMALLY fadm=0, NECESSARY FOR WRITING OF 

TEXTS IN THE PLOT %% 
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for j=0:m 

eAtau=expm(A*j*h/m) ; 

11 CHOOSE e.g 2 :n-3 TO AVOID PROBLEMS AT THE ENDPOINTS 11 
for i*fadm:n-l 

entry( : ,i+l)=eAtau*(x(: ,i+l)+Mtau(: ,2*j+l :2*j+2)* 
Minv*(e_Ah*x( : , i+2)-x( : , i+1))) ; 
csignvec( : , i+l)=B '♦expmC-A' * j *h/m) *Minv* 

(e_Ah*x(: ,i+2)-x(: ,i+l)) ; 
if j—0 
if i==f adm; 

entryl=entry (1 ,fadm+l) ; 
entry2=entry (2,fadm+l) ; 
csignvecl=csignvec(l,fadm+l) ; 
end; 
end ; 

if rem(j ,mp)==0 
if j<=m-mp 

t r a j ect ( 1 , j / mp*n+ ( i+ 1 ) ) =entry ( 1 , i+ 1) ; 
end; 
end; 
if j==m 
if i==n-l 


traject(l , 6*n+l)=entry(l,i+l) ; 
end; 
end; 

if spc_plot==l 

plot (i*h+j*h/m,entry(l J i+ 1 ) , ’ . ’) 
plot (i*h+j*h/m,csignvec(l, i+1) » ' • ’) 
11 ACCELERATION 11 

plot (i*h+j*h/m , lambda*entry (2, i+1) + 

csignvec(l,i + l ) , ' . ') 

elseif spc_plot==2 

plot(i*h+j*h/m,entry(l,i+l) , ' • ’) 

plot (i*h+j*h/m,entry(2 , i+1) , ' • ' ) 

plotCi+h+j+h/m.csignvecCl.i+l) » ’ • ’) 

11 ACCELERATION */.•/, 

plot ( i*h+ j *h/m , lambda*ent ry (2 , i+1) + 


11 TRAJECTORY 11 
11 CONTROL u 11 


11 TRAJECTORY 11 
11 VELOCITY 11 
11 CONTROL u 11 
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csignvec(l,i+l) , ' . ') 
elseif spc_plot==3 

plot(i*h+j*h/m,entry(l,i+l) 

•/.'/. ACCELERATION •/,*/, 
plot (i*h+j*h/m , lambda*entry (2, i+l)+ 
csignvec(l,i+l) , ’ . 
elseif spc_plot==4 

plot (i*h+j*h/m, entry (1, i+1) , ' . ') 
plot (i*h+j*h/m, entry (2, i+1) , ' . >) 
elseif spc_plot==5 

plot (i*h+j*h/m, entry (1 , i+1) , * . ') 
plot(i*h+j*h/m,csignvec(l,i+l ) , ‘ . *) 
else 

disp(' ') 

disp( ' NOT A VALID CHOICE ') 
breakadm=l ; 
end; 

if breakadm 
break; 
end; 
end; 

if breakadm 
break; 
end; 
end; 

if spc_plot<6 
ax=axis ; 
ax2=ax(l ,2) ; 
if ax2<1.5 
xpos=3/4*0.2; 
elseif ax2>=5 
xpos=3/4; 
else 

xpos=0.25; 

end; 

if spc_plot<3 
ax4=ax(l ,4) ; 


XX TRAJECTORY ’/.7. 


1.1. TRAJECTORY VI. 

1.1, VELOCITY VI, 

VI. TRAJECTORY •/.*/. 
VI. CONTROL u VI. 
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if ax4<l 
ax4=ax4*10; 
if ax4<l 
ax4=ax4*10 ; 
end; 
end; 

if ax4>10 
ax4=ax4/ 10; 
if ax4>10 
ax4=ax4/10 ; 
end; 
end; 

if rem(ax4,2)==0 
yadd=(ax(l,4)/4)*l/5; 
else 

yadd=(ax(l ,4)/3)*l/5 ; 
end; 

ypos=lambda*entry2+csignvecl ; 
t ext ( -xpos, ypos, ['ACC']) ; 
if abs(ypos-csignvecl)>yadd 
text(-xpos,csignvecl, ['CS u']); 
elseif ypos-csignvecl>0 

text (-xpos, csignvecl-yadd, ['CS u’]) ; 
else 

text (-xpos, csignvecl+yadd, ['CS u']) ; 
end; 

if spc_plot==2 
text (-xpos, entry2, ['VEL'] ) ; 
end; 
end; 

if spc_plot==3 

ypos=lambda*entry2+csignvecl ; 
text (-xpos , ypos , ['ACC']) ; 
end; 

if spc_plot==4 

text (-xpos, entry2, ['VEL']) ; 
end; 
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if spc_plot==5 

text(-xpos,csignvecl, ['CS u']); 
end; 
end; 


*/.*/. ESTIMATION OF THE ACCURACY IN THE SPLINE 
APPROXIMATION 7.*/. 


if plotadm==0 

[Total_error , Average. error , Po int .error ,End_point .error] = 
spline_error(pointf cn ,R, traject) ; 

Total.error 
Average.error 
Point.error 
End.po int.error 

title( [’One dim. traj : mprl21der0.m lambda * ' ,num2str 
(lambda ) , * n = * ,num2str(n)] ) 

xlabel([ 'Total error : ' ,num2str (Total.error) , ' Point 

error : ' ,num2str(Point_error) , ' Graph : 
num2str(spc_plot) , ' Time t']) 
if spc_plot<6 
for k=0:n 

plot(k*h,x(l,k+l) , 'o') 
end; 
end; 
end; 

plot adm=p lot adm+1 ; 
disp(' ') 

disp( ' WOULD YOU LIKE TO SEE ANOTHER GRAPH OF THE CURRENT 
SYSTEM AND ITS TRAJECTORY ? ') 
disp(' ’) 

disp( ’ FINISH THE PROGRAM : 0 ') 

disp( ' DISPLAY THE TRAJECTORY, CONTROL u AND 

ACCELERATION : 1 ') 

disp( ’ DISPLAY THE TRAJECTORY, VELOCITY, CONTROL u AND 
ACCELERATION : 2 ') 
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dispC' DISPLAY THE TRAJECTORY AND ACCELERATION : 3 ') 
disp( ' DISPLAY THE TRAJECTORY AND VELOCITY : 4 ') 
disp( ’ DISPLAY THE TRAJECTORY AND CONTROL u : 5 ') 
spc.plot = input ( ’ MAKE YOUR CHOICE : '); 
end; 


clear global; 
for k=2:plotadm 
delete (k) ; 
end; 
end; 


function [Q.cnt] = quad812mod(funfcn,a,b,tol) 

/Alteration of the original matlab toolbox program. 

/QUAD8 Numerical evaluation of an integral, higher order 
V, method. Q = QUAD8 ( 'F ' , A ,B ,T0L) approximates the 
/ integral of F(X) from to B to within a relative error 

% of TOL. ’F' is a string containing the name of the 

X function. The function must return a 2*2-matrix 
*/» output value if given an input value. 

% Q = Inf is returned if an excessive recursion level 
L is reached indicating a possibly singular integral. 

*/ QUAD8 uses am adaptive recursive Newton Cotes 8 panel 

•/. rule. 

Z Cleve Moler, 5-08-88. 

*/. Copyright (c) 1984-94 by The MathWorks, Inc. 

/% [Q.cnt] — quad8(F,a,b,tol) also returns a function 
X evaluation count. 

X Top level initialization, Newton-Cotes weights 

w = [3956 23552 -3712 41984 -18160 41984 -3712 23552 
3956] /14175 ; 


x = a + (0:8)*(b-a)/8; 
X set up function call 
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for i=x 

y = [y feval(funfcn, i)] ; 
end; 

*/, Adaptive, recursive Newton-Cotes 8 panel quadrature 
QO = zeros (2) ; 

[Q , cnt] = quad812stpmod(funfcn,a,b,tol,0,w,x,y,q0) ; 

cnt = cnt + 9; 
end ; 


function [Q.cnt] = quad812stpmod(FunFcn,a,b,tol ,lev, 

w,xO,fO,qO) 

/Alteration of the original matlab toolbox program. 
’/QUAD8STP Recursive function used by qUAD8. 

*/. [q,cnt] = quad8stp(F,a,b,tol,lev,w,f ,q0) tries to 
'/• approximate the integral of f(x) from a to b to 
/ within a relative error of tol. F is a string 
'/, containing the name of f . The remaining arguments 
'!• are generated by quad8mod or by the recursion. 

X lev is the recursion level. 

/ w is the weights in the 8 panel Newton Cotes formula. 
*/. xO is a vector of 9 equally spaced abscissa is the 
7 . interval . 

/ fO is a matrix of the 9 function values at x. 

*/. qo is an approximate value of the integral. 

'/ Cleve Moler, 5-08-88. 

*/. Copyright (c) 1984-94 by The MathWorks, Inc. 

LEVMAX = 10; 

*/ Evaluate function at midpoints of left and 
right half intervals, 
x = zeros(l,17); 
x(l :2 : 17) = xO; 

x(2 : 2 : 16) = (x0(l:8) + x0(2:9))/2; 
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f ( : , 1 : 2) = f0(:,l:2); 
for i=l:8 

f ( : ,4*i-l :4*i) = feval(FunFcn,x(2*i)) j 
f (: ,4*i+l:4*i+2) = (f 0( : ,2*i+l : 2*i+2)) ; 
end; 

7, Integrate over half intervals, 
h = (b-a)/16; 

Q1 = 0 ; Q2=0 ; 
for i=l:9 

Q1 = Q1 + h*w(i)*f ( : ,2*i-l :2*i) ; 

Q2 = Q2 + h*w(10-i)*f ( : ,35-i*2:36-i*2) ; 
end; 

Q = qi + Q2 ; 

V, Recursively refine approximations, 
if norm(Q - QO) > tol*norm(Q) & lev <= LEVMAX 
c = (a+b)/2; 

[Ql,cntl] = quad812stpmod(FunFcn , a, c , tol/2 , lev+1 , 

w,x(l:9),f(:,l: 18) ,Q1) ; 
Cq2,cnt2] = quad812stpmod(FunFcn,c,b, tol/2, lev+1, 

w,x(9:17),f(:, 17:34), Q2); 

q = qi + q 2 ; 

cnt = cnt + cntl + cnt2; 

end 

end; 
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function x = mprl41knovel(spc_plot,t,n,cleargr, pointfcn, 
lambda 1 , lambda2 , ep) 


•/.*/. THIS PROGRAM CALCULATES CONTROLLAWS FOR A ONE 
DIMENSIONAL TRAJECTORY ’/.*/. 

VL spc.plot DETERMINES WHICH GRAPH TO BE DISPLAYED, 
CHOSE AN INTEGER =<9 11 

VL t IS THE TIME PERIOD FOR WHICH THE SYSTEM IS 
TO BE CONTROLLED 'Ll 

VI. n ARE THE NUMBER OF POINTS AT THE SPECIFIED 
TRAJECTORY, CHOSE t/n>l/10 11 
11 cleargr CLEARS THE CURRENT WINDOW, CHOSE 1 OR 0 11 

11 pointfcn SPECIFIES THE TRAJECTORY 11 
VL lambdal AFFECTS THE INSTABILITY OF THE SYSTEM 11 
11 lambda2 ALSO EFFECTS THE STABILITY OF THE SYSTEM, 
CHOSE 11--12 11 

VL ep<>0 PUTS A ZERO IN THE TRANS FERFUNCTI ON 11 

global A B h t n 

11 THE SYSTEM 11 

A=[ 0 1 0 0; 

0 lambdal 1 0; 

0 0 0 1 ; 

000 lambda2] ; 


B=[ 0 ep 0 1] ’ ; 


C= [ 1 0 0 0] ; 


11 FUNCTION pointfcn DETERMINES THE SPECIFIED TRAJECTORY 11 

11 NECESSARY FOR THE COMPOUND FUNCTION pointsinl2 11 

if pointfcn= =, pointsinl2' ; 
t=5.2; 
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n=26 ; 
end; 

R=feval(pointf cn) ; 

•/,•/. CALCULATION OF THE INTEGRAL FROM 0 TO h 7 . 7 . 

m=48; 7 . 7 . NUMBER OF POINTS BETWEEN INTERPOLATION, 

CHOSE A MULTIPLE OF 6 7 . 7 . 

mp=m/6 ; 7 . 7 . NEEDED FOR fen spline.error THAT 

DETERMINES THE PRECISION IN THE 
SPLINE APPROXIMATION 7 . 7 . 

tol=le-08 ; 7 . 7 . THE NUMERIC ERROR TOLERANCE 7 . 7 . 

Mtau( : ,1 : 4) =zeros (4) ; 
tau=0 ; 
for j=l:m 
oldtau=tau; 
tau=oldtau+h/m ; 

Mtau(: ,4*j+l :4*j+4)= quad814mod( ’ integrand’ ,oldtau,tau,tol) 
+ Mtau( : ,4*j-3:4*j) ; 
end; 

M=Mtau( : ,4*m+l :4*m+4) ; 


7 . 7 . FORMING OF THE MATRICES FOR THE BLOCKDIAGONAL SYSTEM 7 . 7 . 

e_Ah=expm(-A*h) ; 

Minv=inv(M) ; 

ZZ=Minv*e_Ah; 

WW=e_Ah ' *ZZ+Minv ; 


7 . 7 . CONTINUOUS CONTROLLAW 7 . 7 . 

WLDO= [ep*WW (2,2) +WW (4,2) ep*WW(2 ,3)+WW(4,3) ep*WW(2,4)+ 
WW(4,4)]; 

ZLUDO= [ep*ZZ (2 , 2) +ZZ (4 , 2) ep*ZZ(2 ,3)+ZZ(4,3) ep*ZZ(2,4)+ 
ZZ(4,4)] ; 
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ZLLD0= [ep*ZZ(2 ,2)+ZZ(2 ,4) ep*ZZ(3,2)+ZZ(3,4) ep*ZZ(4,2)+ 
ZZC4.4)]; 

WRD0=[ep*WW(2,l)+WW(4, 1)] ; 

ZRUD0= [ep*ZZ(2 , 1)+ZZ(4, 1)] ; 

ZRLD0= [ep*ZZ(l ,2)+ZZ(l ,4)] ; 

7 . 7 . CONTINUOUS FIRST DIFFERENTIAL OF CONTROLLAW */.•/, 

WLD1= [ep*WW (1,2) +ep*lambdal*WW(2 , 2) +WW(3, 2)+lambda2*WW(4, 2) 
ep*WW(l , 3)+ep*lambdal*WW(2,3)+WW(3,3)+lambda2*WW(4,3) 
ep*WW (1,4) +ep*lambdal*WW ( 2 , 4) +WW (3 , 4) +lambda2*WW (4,4)]; 

ZLUD 1= [ep*ZZ ( 1 , 2) +ep*lambdal*ZZ (2 , 2) +ZZ (3 , 2) +lambda2*ZZ (4 , 2) 
ep*ZZ ( 1 , 3)+ep*lambdal*ZZ (2 , 3) +ZZ (3 , 3) +lambda2*ZZ (4 , 3) 
ep*ZZ(l,4)+ep*lambdal*ZZ(2,4)+ZZ(3,4)+lambda2*ZZ(4,4)] ; 

ZLLD 1 = [ep*ZZ (2,1) +ep*lambdal *ZZ (2 , 2) +ZZ (2 , 3) +lambda2*ZZ (2 , 4) 
ep*ZZ (3,1) +ep*lambdal*ZZ (3 , 2) +ZZ (3 , 3) +lambda2*ZZ (3 , 4) 
ep*ZZ (4 , 1) +ep*lambdal*ZZ (4 , 2) +ZZ (4 ,3)+lambda2*ZZ (4,4)] ; 
WRDl=[ep*WW(l,l)+ep*lambdal*WW(2, l)+WW(3,l)+lambda2*WW(4,l)] ; 

ZRUD 1= [ep*ZZ (1,1) +ep*lambdal *ZZ (2 , 1 ) +ZZ (3 , 1) +lambda2*ZZ (4,1)]; 

ZRLD 1= [ep*ZZ (1,1) +ep*lambdal *ZZ ( 1 , 2) +ZZ (1,3) +lambda2*ZZ (1,4)]; 

7 . 7 . CONTINUOUS SECOND DIFFERENTIAL OF CONTROLLAW 7 . 7 . 

WLD2= [ep*lambdal*WW ( 1 , 2) + (ep*lambdal ~2+l) *WW(2 , 2) +lambda2* 
WW(3,2)+lambda2~2*WW(4,2) ep*lambdal*WW(l ,3)+ 
(ep*lambdal~2+l)*WW(2,3)+lambda2*WW(3,3)+lambda2“2* 
WW(4,3) ep*lambdal*WW(l,4)+(ep*lambdal~2+l)*WW(2,4)+ 
lambda2*WW(3,4)+lambda2~2*WW(4,4)] ; 

ZLUD2= [ep*lambdal*ZZ (1 , 2) + (ep*lambdal‘'2+l) *ZZ(2 , 2) +lambda2* 

ZZ (3 , 2) +lambda2“2*ZZ (4,2) ep*lambdal*ZZ (1,3)+ 
(ep*lambdal~2+l)*ZZ(2,3)+lambda2*ZZ(3,3)+ 
lambda2~2*ZZ(4,3) ep*lambdal*ZZ(l,4)+(ep*lambdal~2+l)* 
ZZ(2 ,4)+lambda2*ZZ(3,4)+lambda2“2*ZZ(4,4)] ; 
ZLLD2=[ep*lambdal*ZZ(2,l)+(ep*lambdal~2+l)*ZZ(2,2)+lambda2* 
ZZ(2,3)+lambda2~2*ZZ(2,4) ep*lambdal*ZZ(3, 1)+ 
(ep*lambdal“2+l)*ZZ(3,2)+lambda2*ZZ(3,3)+lambda2~2* 
ZZ(3,4) ep*lambdal*ZZ(4, l)+(ep*lambdal~2+l)*ZZ(4,2)+ 
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lambda2*ZZ(4,3)+lambda2~2*ZZ(4,4)] ; 
WRD2=[ep*lambdal*WW(l, l) + (ep*lambdal' , 2+l)*WW(2,l)+lambda2* 

WW(3 J l)+lambda2"2*WW(4,l)] 5 

ZRUD2= [ep*lambdal*ZZ(l , l)+(ep*lambdal"2+l)*ZZ(2, l)+lambda2* 
ZZ(3,l)+lambda2‘2*ZZ(4,l )] ; 

ZRLD2=[ep*lambdal*ZZ(l,l)+(ep*lambdal~2+l)*ZZ(l,2)+lambda2* 
ZZ(1 ,3)+lambda2~2*ZZ(l,4)] j 

y.y, DIFFERENTIAL APPROXIMATIONS FOR THE BOUNDARY 
CONDITIONS y.y. 

ydlO=(R(l ,2)-R(l , l))/h; 
ydll=(R(l ,3)-R(l ,2) )/h; 
ydl2=(R(l ,4)-R(l ,3) )/h; 
ydln=(R(l ,n+l) -R(l ,n) ) /h ; 
ydln_l=(R(l ,n)-R(l ,n-l) )/h; 
ydln_2=(R(l ,n-l)-R(l ,n-2) )/h; 
uO= CydlO-ydl 1) /h ; 
ul=(ydll-ydl2)/h; 
un=(ydln_l-ydln)/h; 
un_l=(ydln_2-ydln_l)/h; 
udlO=(uO-ul) /h; 
udln=(un_l-un)/h; 

X0= [ydlO ; uO; udlO] ; */.•/. 3/4 of the the first state vector '/.*/. 
Xn= [yd In; un; udln] ; */,'/. 3/4 of the the last state vector '/.'/. 

*/.*/. FORMING OF THE RIGHT HAND SIDE OF THE 
BLOCKDIAGONAL SYSTEM */.•/. 

j*i; 

for i=2:n 

OmegaCj , l)=-ZRLDO*R(l , i-l)+WRDO*R(l , i) -ZRUDO*R(l , i+1) ; j-j+1; 
OmegaCj ,l)=-ZRLDl*R(l,i-l)+WRDl*R(l,i)-ZRUDl*R(l,i+l) ; j=j+l; 
Omega(j , l)=-ZRLD2*R(l , i-l)+WRD2*R(l , i)-ZRUD2*R(l, i+1) ;j=j+l; 
end; 

Omega (1 , l)=Omega(l , 1)-ZLLD0*X0; 

Omega (2 , l)=0mega(2, 1)-ZLLD1*X0; 
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0mega(3, l)=0mega(3, 1)-ZLLD2*X0; 

Omega (3*n-5 , l)=0mega(3*n-5, l)-ZLUDO*Xn; 
Omega (3*n-4, l)=Omega(3*n-4, l)-ZLUDl*Xn; 
0mega(3*n-3, l)=0mega(3*n-3, l)-ZLUD2*Xn; 

7.7. FORMING OF THE LEFT HAND SIDE OF THE 
BLOCKDIAGONAL SYSTEM 7.7. 

for i=l:3 

DD(l,i)*-WLDO(l,i); 

DD(2,i)=-WLDl(l,i) ; 

DD(3,i)=-WLD2(l,i) ; 
if n>2 

DD(1 , i+3)=ZLUD0 (1 , i) ; 

DD (2 , i+3)=ZLUDl (1 , i) ; 

DD(3 , i+3)=ZLUD2 (1 , i) ; 
end; 
end; 

for i=2 : n-2 
for j=-l : 1 

DD(3*i-2 ,3*i-4+j )=ZLLD0(1 , j+2) ; 
DD(3*i-2 ,3*i-l+j )=-HLDO(l , j+2) ; 
DD(3*i-2,3*i+2+j )=ZLUD0(1 , j+2) ; 
DD(3*i-l ,3*i-4+j )=ZLLD1 (1 , j+2) ; 
DD(3*i-l ,3*i-l+j )=-WLDl (1 , j+2) ; 
DD(3*i-l ,3*i+2+j )=ZLUD1 (1 , j+2) ; 
DD(3*i ,3*i-4+j)=ZLLD2(l > j+2) ; 
DD(3*i ,3*i-l+j )=-WLD2(l , j+2) ; 
DD(3*i ,3*i+2+j)=ZLUD2(l, j+2) ; 
end; 
end; 
if n>2 

for i=l:3 

DD(3*n-5,3*n-9+i)=ZLLD0(l,i) ; 

DD (3*n-4 , 3*n-9+i ) =ZLLD1 ( 1 , i) ; 
DD(3*n-3, 3*n-9+ i ) =ZLLD2 ( 1 , i ) ; 
DD(3*n-5,3*n-6+i)=-WLD0(l,i) ; 
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DD(3*n-4,3*n-6+i)=-WLDl(l,i); 
DD(3*n-3,3*n-6+i)=-WLD2(l,i) ; 
end; 
end; 

DD=sparse(DD) ; %% SQUEEZING OUT ALL ZERO ELEMENTS 

FROM MATRIX DD VI. 

VI. GAUSSELIMINATION TO PRODUCE AN UPPER TRIANGULAR 
SYSTEM V/. 


for k=2:3:3*n-7 
for l=k:k+4 

if DD(k-l ,k-l)"=0 

zd=DD(l ,k-l)/DD(k-l ,k-l) ; 

DD(1 , : )=DD(1 , : )-zd*DD(k-l , : ) ; 

OmegaCl, l)=Omega(l, 1 ) -zd* Omega (k- 1,1) ; 
end; 
end; 

for l=k+l :k+4 
if DD(k,k)"=0 

zd=DD(l,k)/DD(k,k) ; 

DD(1, :)=DD(1, :)-zd*DD(k, :); 

Omega (1,1) =Omega (1 , 1 ) -zd*0mega (k , 1) ; 
end; 
end; 

for l=k+2:k+4 

if DD(k+l ,k+l)~=0 

zd=DD(l,k+l)/DD(k+l,k+l) ; 

DD (1 , : )=DD(1 , : ) -zd*DD(k+l , : ) ; 

OmegaCl , l)=Omega(l , l)-zd*Omega(k+l , 1) ; 
end; 
end; 
end; 

k=3*n-5 ; 
if DD(k,k) ~=0 

zd=DD (k+1 ,k) /DD (k , k) ; 
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DD(k+l, :)=DD(k+l, :)-zd*DD(k, :); 

Omega (k+ 1,1) =Omega (k+ 1 , 1 ) -zd*Omega (k , 1 ) ; 
zd=DD(k+2,k)/DD(k,k) ; 

DD (k+2 , : ) =DD (k+2 , : ) -zd*DD (k , : ) ; 

0mega(k+2, l)=0mega(k+2, l)-zd*Omega(k, 1) ; 
end; 

if DD(k+l ,k+l) ”=0 

zd=DD(k+2,k+l)/DD(k+l ,k+l) ; 

DD (k+2 , : ) =DD (k+2 , : ) -zd*DD (k+ 1 , : ) ; 
0mega(k+2,l)=0mega(k+2, l)-zd*Omega(k+l,l) ; 
end; 

7 . 7 . BACKSUBSTITUTION TO SOLVE FOR THE STATEVECTORS 7 . 7 . 


udl (n-l)=0mega(3*(n-l) , l)/DD(3*(n-l) ,3* (n-1)) ; 
u (n- 1 ) = (Omega (3*n-4 , 1 ) -DD (3*n-4 , 3* (n-1 ) ) *udl (n-1) ) / 
DD(3*n-4,3*n-4) ; 

ydl (n- 1 ) = (0mega(3*n-5 , 1 ) -DD (3*n-5 , 3* (n-1) ) *udl (n- 1 ) - 
DD (3*n-5 , 3*n-4) *u (n- 1) ) /DD ( 3*n-5 , 3*n-5) ; 
if n>2 

for k=n-2 : -1 : 1 

ud 1 (k) = ( Omega (3*k , 1 ) -DD (3*k , 3*k+3) *udl (k+1) - 
DD (3*k , 3*k+2) * u (k+1) -DD (3*k, 3*k+l) *ydl (k+1) ) / 
DD(3*k,3*k) ; 

u(k)=(0mega(3*k-l , l)-DD(3*k-l ,3*k+3)*udl(k+l)- 
DD(3*k-l ,3*k+2)*u(k+l)-DD(3*k-l ,3*k+l)*ydl(k+l)- 
DD (3*k- 1 , 3*k) *udl (k) ) /DD (3*k- 1 , 3*k-l) ; 
yd 1 (k) = (Omega (3*k- 2 , 1 ) -DD (3*k-2 , 3*k+3) *udl (k+ 1 ) - 
DD (3*k-2 , 3*k+2) *u(k+l ) -DD (3*k-2 , 3*k+l ) *ydl (k+1) 
-DD (3*k-2 , 3*k) *udl (k) -DD (3*k-2 , 3*k-l) *u(k) )/ 
DD(3*k-2 ,3*k-2) ; 
end; 
end; 


7 . 7 . MAKING OF THE STATEVECTORS 7 . 7 . 


x(:,l)-[R(l,l); XO]; 
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x(: ,n+l)=[R(l, n +l); Xn] ; 
for i=l :n-l 

x(: ,i+l)*[R(l,i+l) ; ydl(i); u(i) ; udl(i)]; 
end; 

if cleargr 
elf ; 

hold on; 
grid on; 
end; 


'/.y. PLOTTING OF THE CALC/SPEC TRAJECTORY, VELOCITY, 
CONTROLS IGNAL AND ACCELERATION %y. 

plotadm=0; 
while spc_plot 
if plotadm 

if spc_plot<10 
figure; 
hold on; 
grid on; 

title( ['One dim. traj : mprl41knovel .m 11 = 

’ ,num2str(lambdal) , ' 12 = ’ ,num2str(lambda2) , ’ ep = 

’ ,num2str(ep) , ’ n = ' ,num2str(n)] ) 

xlabelC ['Total error : » ,num2str(Total_error) , 

Point error : ' ,num2str(Point_error) , ' Graph : 
' ,num2str(spc_plot) , ' Timet']) 
if spc_plot<6 
for k=0:n 

plot (k*h,x(l ,k+l) , 'o') 
end; 
end; 
end; 

end; 

breakadm=0 ; 

fadm=0; */,*/ % NORMALLY fadm=0, NECESSARY FOR WRITING 
OF TEXTS IN THE PLOT V/. 
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for j=0:m 

eAtau=expm(A*j*h/m) ; 

for i=f adm:n-l CHOOSE e.g 2:n-3 TO AVOID 

PROBLEMS AT THE ENDPOINTS 11 
entryC : ,i+l)=eAtau*(x(: ,i+l)+Mtau(: ,4*j+l :4*j+4)* 
Minv*(e_Ah*x( : ,i+2)-x(: ,i+l))) ; 

csignvec ( : , i+1 ) =B ' *expm(-A ' * j *h/m)*Minv* (e_Ah*x ( : , i+2) - 
x( : , i+1) ) ; 
if j==0 

if i==f adm; 

entryl=entry(l,fadm+l) ; 
entry2=entry (2,fadm+l) ; 
entry3=entry (3,fadm+l) ; 
entry4=entry(4,fadm+l) ; 
csignvecl=csignvec(l,fadm+l) ; 
end; 
end; 

if rem(j ,mp)==0 
if j<=m-mp 

t r a j ect ( 1 , j / mp*n+ ( i+ 1 ) ) =entry ( 1 , i+ 1) ; 
end; 
end; 
if j ==m 
if i==n-l 

traj ect (1 , 6*n+l)=entry(l , i+1) ; 
end; 
end; 

if spc_plot==l 

plot (i*h+j*h/m, entry(l , i+1) , ’ . ’ ) H TRAJECTORY */.•/. 
plot (i*h+j*h/m, entry (3, i+1) , ' . ’) 11 CONTROL u 11 

11 ACCELERATION 11 

plot (i*h+j*h/m > lambdal*entry(2, i+l)+ 
entry (3, i+l)+ep*csignvec(l , i+1) 
elseif spc_plot==2 

plot (i*h+j*h/m, entry ( l,i+l) ’) H TRAJECTORY 11 
plot(i*h+j*h/m,entry(2,i+l) , ’ . ’) 11 VELOCITY 11 

plot (i*h+j*h/m, entry (3, i+1) , ’ . ') 11 CONTROL u 11 
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plot (i*h+j*h/m,lambdal*entry(2,i+l)+entry(3,i+l)+ 
ep*csignvec(l , i+1) , » . >) 7 . 7 . ACCELERATION 7 ,*/. 

elseif spc_plot==3 

plot(i*h+j*h/m,entry(l,i+l) , ' . ’) 7 . 7 . TRAJECTORY •/.*/. 

plot (i*h+j*h/m,lambdal*entry(2, i+l)+entry(3, i+l)+ 
ep*csignvec(l , i+1) , » . » ) 7 . 7 . ACCELERATION 7 . 7 . 

elseif spc_plot==4 

plot (i*h+j*h/m, entry (1, i+1) ») %% TRAJECTORY 7 , 7 , 

plot(i*h+j*h/m,entry(2,i+l) , ' . ') 7 . 7 . VELOCITY 7 . 7 . 

elseif spc_plot==5 

plot (i*h+j *h/m, entry (1 , i+1) , ' . ’ ) 7 , 7 , TRAJECTORY 7 . 7 . 

plot(i*h+j*h/m,entry(3,i+l) , ’ . ') 7 . 7 . CONTROL u •/.*/, 

elseif spc_plot==6 

plot (i*h+j*h/m, entry (4, i+1) 7 . 7 . CONTROL DER 

u-dot 7 , 7 , 

elseif spc_plot==7 

cs ignvec ( : , i+1 ) =B ' *expm (- A ’ * j *h/m) *Minv* 

(e_Ah*x( : ,i+2)-x( : , i+1) ) ; 

7 . 7 . CONTROLSIGNAL w 7 . 7 , 
plot(i*h+j*h/m,csignvec(l ,i+l) , ' . ') 
elseif spc_plot==8 

csdlvec ( : , i+1) =B ' *A ’ *expm(~A 1 *j*h/m)*Minv* 

(e_Ah*x( : ,i+2)-x(: ,i+l)); 

•/.*/. CONTROL DER w-dot 7 . 7 . 

plot (i*h+j*h/m,csdlvec(l, i+1) , > . ») 

if j==0 

csdvecl=csdlvec(l , 1) ; 
end; 

elseif spc_plot==9 

csd2vec( : J i+l)=B , *A , *A'*expm(-A , *j*h/m)* 

Minv*(e_Ah*x( : , i+2)-x( : , i+1) ) ; 

7 . 7 . CONTROL DERx2 w-dot-dot 7 . 7 . 
plot (i*h+j*h/m,csd2vec(l, i+1 ) , • . ») 
if j ==0 

csdvec2=csd2vec(l , 1) ; 
end; 
else 
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disp(' ') 

disp( ' NOT A VALID CHOICE ') 
breakadm=l ; 
end; 

if breakadm 
break; 
end; 
end; 

if breakadm 
break; 
end; 
end; 

if spc_plot<10 
ax = ax is ; 
ax2=ax(l ,2) ; 
if ax2<1.5 
xpos=3/4*0.2; 
elseif ax2>=5 
xpos=3/4; 
else 

xpos=0.25; 

end; 

if spc_plot<3 
ax4=ax(l,4) ; 
if jlx4<1 
ajc4=ax4*10; 
if ax4<l 
2Lx4=ax4*10; 
end; 
end; 

if ax4>10 
ax4=ax4/ 10; 
if ax4>10 
2lx4=2lx4/10 ; 
end; 
end; 

if rem(ax4,2)==0 
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yadd=(ax(l,4)/4)*l/5; 

else 

yadd=(ax(l,4)/3)*l/5; 

end; 

ypos=lambdal*entry2+entry3+ep*csignvecl ; 
t ext ( -xpos, ypos, ['ACC']) ; 
if abs(ypos-entry3)>yadd 
text(-xpos,entry3, ['CS u']); 
elseif ypos-entry3>0 

text(-xpos,entry3-yadd, ['CS u']) ; 
else 

text (-xpos, entry3+yadd, ['CS u']) ; 
end; 

if spc_plot==2 
text (-xpos, entry2, ['VEL']); 
end; 
end; 

if spc_plot==3 

ypo s = 1 ambd a 1 * ent r y 2 +ent r y 3+ ep * cs ignvec 1 ; 
text (-xpos, ypos, ['ACC']) ; 
end; 

if spc_plot==4 
text (-xpos , entry2 , ['VEL']) ; 
end; 

if spc_plot==5 

text (-xpos, entry3, ['CS u']); 
end; 

if spc_plot==6 
t ext (-xpos, ent ry4, ['udl']) ; 
end; 

if spc_plot==7 

text (-xpos, csignvecl, ['CS w']); 
end; 

if spc_plot==8 

text (-xpos ,csdvecl , [' wdl '] ) ; 
end; 

if spc_plot==9 
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text(-xpos,csdvec2, ['wd2']) ; 
end; 
end; 


n ESTIMATION OF THE ACCURACY IN THE 
SPLINE APPROXIMATION tl 

if plotadm==0 

[Tot al.error , Average_error , Point_error , 

End_po int.error] =spli ne.error (pointf cn ,R , traj ect ) ; 

Total_error 

Average. error 

Point.error 

End_point_error 

title(['0ne dim. traj: mprl41knovel.m 11 = 

' ,num2str(lambdal ) , ' 12 = ' ,num2str(lambda2) , 

' ep =' ,num2str(ep) , ' n = ' ,num2str(n)]) 
xlabel( ['Total error : ' ,num2str(Total_error) , 

' Point error : ' ,num2str(Point_error) , ' Graph : 
' ,num2str(spc_plot) , ' Timet']) 
if spc_plot<6 
for k=0 :n 

plot (k*h,x(l ,k+l) , ’o') 
end; 
end; 
end; 

plotadm=plotadm+l ; 
disp(’ ') 

dispC ' FOLLOWING OPTIONS ARE AVAILABLE : ') 
disp(’ ') 

disp( ’ FINISH THE PROGRAM : 0 ') 

disp( ' DISPLAY THE TRAJECTORY, CONTROL u AND 

ACCELERATION : 1 ') 

disp( ' DISPLAY THE TRAJECTORY, VELOCITY, CONTROL u AND 
ACCELERATION : 2 ') 

disp( ’ DISPLAY THE TRAJECTORY AND ACCELERATION : 3 ’) 
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disp(' DISPLAY THE TRAJECTORY AND VELOCITY : 4 ') 
dispO DISPLAY THE TRAJECTORY AND CONTROL u : 5 ’) 
disp(’ DISPLAY THE FIRST DERIVATIVE OF THE CONTROL 
u : 6 J ) 

disp( ' DISPLAY THE CONTROLSIGNAL w : 7 0 
dispC’ DISPLAY THE FIRST DERIVATIVE OF THE CONTROL 
w : 8 ') 

disp( ' DISPLAY THE SECOND DERIVATIVE OF THE CONTROL 
w : 9 ’) 

spc.plot = input ( ' MAKE YOUR CHOICE : ’); 
end; 


clear global; 
for k=2:plotadm 
delete(k) ; 
end; 
end ; 


function [Q,cnt] » quad814mod(funfcn,a,b,tol) 

XAlteration of the original mat lab toolbox program. 

XQUAD8 Numerical evaluation of an integral, higher order 
X method. Q = QUAD8('F' ,A,B,T0L) approximates the 
X integral of F(X) from to B to within a relative error 

X of TOL. 'F* is a string containing the name of the 

X function. The function must return a 4*4-matrix 
X output value if given an input value. 

X Q = Inf is returned if an excessive recursion level 
X is reached indicating a possibly singular integral. 

X QUAD8 uses an adaptive recursive Newton Cotes 8 panel 

X rule. 

X Cleve Moler , 5-08-88. 

X Copyright (c) 1984-94 by The MathWorks, Inc. 

X [Q,cnt] = quad8(F,a,b,tol) also returns a function 

X evaluation count. 

X Top level initialization, Newton-Cotes weights 

w = [3956 23552 -3712 41984 -18160 41984 -3712 23552 
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3956]/14175; 

x = a + (0:8)*(b-a)/8; 

7. set up function call 
for i=x 

y = Cy feval(funf cn, i)] ; 
end; 

7. Adaptive, recursive Newton-Cotes 8 panel quadrature 
QO = zeros (4) ; 

[Q.cnt] = quad814stpmod(funfcn,a,b,tol,0,w,x,y,Q0) ; 

cnt = cnt + 9; 

end; 


function [Q,cnt] = quad814stpmod(FunFcn,a,b,tol,lev, 

w,xO,fO,QO) 

^Alteration of the original matlab toolbox program 
5CQUAD8STP Recursive function used by QUAD8. 

7, [Q,cnt] = quad8stp(F,a,b,tol,lev,w,f ,Q0) tries to 
7* approximate the integral of f(x) from a to b to 
7, within a relative error of tol . F is a string 
7t containing the name of f . The remaining arguments 
7» are generated by quad8mod or by the recursion. 

7, lev is the recursion level. 

7% w is the weights in the 8 panel Newton Cotes formula. 

7. xO is a vector of 9 equally spaced abscissa is the 

7. interval. 

51 fO is a matrix of the 9 function values at x. 

7, qo is an approximate value of the integral. 

7. Cleve Moler, 5-08-88. 

*/. Copyright (c) 1984-94 by The MathWorks, Inc. 

LEVMAX =10; 

7. Evaluate function at midpoints of left and 
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right half intervals, 
x = zeros(l , 17) ; 
x(l : 2 : 17) = xO; 

x(2 : 2 : 16) = (x0(l:8) + x0(2:9))/2; 

f ( : ,1:4)= fO(: ,1:4); 
for i=l:8 

f ( : ,8*i-3:8*i) = feval(FunFcn,x(2*i)) ; 
f c : ,8*i+l :8*i+4) = (f0( : ,4*i+l :4*i+4)) ; 
end; 

7, Integrate over half intervals, 
h = (b-a)/l6; 

Q1=0;Q2=0; 
for i=l:9 

Q1 = Q1 + h*w(i)*f ( : ,4*i-3 :4*i) ; 

Q2 » Q2 + h*w(10-i)*f (: ,69-i*4: 72-i*4) ; 
end ; 

Q = Q1 + Q2; 

7,7, Recursively refine approximations, 
if norm(q - QO) > tol*norm(Q) k lev <= LEVMAX 
c = (a+b)/2; 

[Ql,cntl] = quad814stpmod(FunFcn,a,c,tol/2,lev+l, 

w,x(l:9),f(:, 1:36) ,Q1) ; 
[Q2,cnt2] = quad814stpmod(FunFcn,c,b,tol/2,lev+l, 

w, x(9:17),f(:, 33:68), Q2); 

q = qi + q2; 

cnt = cnt + cntl + cnt2; 

end 

end; 
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function x = mprl51knovel (spc_plot ,t ,n,cleargr,pointf cn, 
lambdal , lambda2 , ep) 

7 . 7 . THIS PROGRAM CALCULATES CONTROLLAWS FOR A ONE 
DIMENSIONAL TRAJECTORY 7.V. 

7 . 7 . spc.plot DETERMINES WHICH GRAPH TO BE DISPLAYED, 
CHOSE AN INTEGER =<11 7 , 7 , 

7 . 7 . t IS THE TIME PERIOD FOR WHICH THE SYSTEM IS 
TO BE CONTROLLED 7 . 7 . 

7 . 7 . n ARE THE NUMBER OF POINTS AT THE SPECIFIED 
TRAJECTORY, CHOSE t/n>l/10 7,7, 

7 . 7 . cleargr CLEARS THE CURRENT WINDOW, CHOSE 1 OR 0 7 . 7 . 

7 . 7 . pointfcn SPECIFIES THE TRAJECTORY 7 . 7 . 

7 . 7 . lambdal AFFECTS THE INSTABILITY OF THE SYSTEM 7 , 7 , 

7 . 7 . USE lambdalOO TO AVOID NUMERICAL PROBLEMS WHEN 
DETERMINE THE MATRIX WW 7 . 7 . 

7 . 7 . Iambda2 ALSO EFFECTS THE STABILITY OF THE SYSTEM, 
CHOSE 11"=12 7 . 7 . 

7 . 7 . ep<>0 PUTS A ZERO IN THE TRAN S FERFUNCTI ON 7 . 7 . 

global A B h t n 

7 . 7 . THE SYSTEM 7 . 7 . 

A=[ 0 1 0 0 0; 

0 lambdal 100; 

00010 ; 

00001 ; 

0 0 0 0 lambda2] ; 


B=[ 0 ep 0 0 1] ‘ ; 


C=[ 1 0 0 0 0]; 

7 . 7 . FUNCTION pointfcn DETERMINES THE SPECIFIED TRAJECTORY 7 . 7 . 

7 . 7 . NECESSARY FOR THE COMPOUND FUNCTION pointsinl2 7 . 7 , 
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if pointfcn==’pointsinl2' ; 
t=5 . 2 ; 
n=26 ; 
end; 

R=feval (pointfcn) ; 

*/.*/. CALCULATION OF THE INTEGRAL FROM 0 TO h Vf, 

m=48; •/,•/, NUMBER OF POINTS BETWEEN INTERPOLATION, 

CHOSE A MULTIPLE OF 6 •/.*/. 

mp=m/6; */.'/. NEEDED FOR fen spline_error THAT DETERMINES 

THE PRECISION IN THE SPLINE APPROXIMATION •/.’/. 
tol=le-08; •/,•/. THE NUMERIC ERROR TOLERANCE VI, 

Mtau( : , 1 :5)=zeros(5) ; 
tau=0; 
for j=l :m 
oldtau=tau; 
tau=oldtau+h/m; 

Mtau(: ,5*j+l :5*j+5)= quad815mod( ’ integrand' ,oldtau,tau,tol) 

+ Mtau( : ,5*j-4:5*j) ; 
end; 

M=Mtau( : ,5*m+l :5*m+5) ; 

VI. FORMING OF THE MATRICES FOR THE BLOCKDIAGONAL SYSTEM VI. 

e_Ah=expm(-A*h) ; 

Minv=inv(M) ; 

ZZ=Minv*e_Ah; 

WW=e_Ah ’ *ZZ+Minv ; 

VI. CONTINUOUS CONTROLLAW •/.*/. 


WLD0= [ep*WW(2 , 2) +WW (5 , 2) ep*WW(2,3)+WW(5,3) 

ep*WW (2,4) +WW (5,4) ep*WW(2,5)+WW(5,5)] ; 
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ZLUDO- [ep*ZZ (2 , 2) +ZZ (5 , 2) ep*ZZ (2 , 3) +ZZ(5 , 3) 

ep*ZZ(2,4)+ZZ(5 ,4) ep*ZZ(2,5)+ZZ(5,5)] ; 

ZLLD0= [ep*ZZ (2 , 2) +ZZ (2 , 5) ep*ZZ (3 , 2) +ZZ(3 , 5) 

ep*ZZ(4,2)+ZZ(4,5) ep*ZZ(5,2)+ZZ(5,5)] ; 

WRD0= [ep*WW(2 , 1)+WW(5, 1)] ; 

ZRUD0= [ep*ZZ(2 , 1)+ZZ(5 , 1)] ; 

ZRLD0= [ep*ZZ ( 1 , 2 ) +ZZ ( 1 , 5 ) ] ; 

7 . 7 , CONTINUOUS FIRST DIFFERENTIAL OF CONTROLLAW */,•/, 
WLDl=[ep*WW(l,2)+ep*lambdal*WW(2,2)+WW(4,2)+lambda2*WW(5,2) 

ep*WW (1,3) +ep*lambdal *WW ( 2 , 3) +WW(4 , 3) +lambda2*WW(5 , 3) 
ep*WW (1,4) +ep*lambdal*WW (2 , 4) +WW (4 , 4) +lambda2*WW(5 , 4) 
ep*WW(l , 5)+ep*lambdal*WW(2,5)+WW(4,5)+lambda2*WW(5,5)] ; 

ZLUD1= [ep*ZZ ( 1 , 2)+ep*lambdal*ZZ (2 , 2) +ZZ(4 , 2) +lambda2*ZZ (5 , 2) 
ep*ZZ ( 1 , 3) +ep*lambdal*ZZ (2 , 3) +ZZ (4 , 3) +lambda2*ZZ (5,3) 
ep*ZZ (1,4) +ep*lambdal *ZZ (2 , 4) +ZZ (4 , 4) +lambda2*ZZ (5,4) 
ep*ZZ(l , 5)+ep*lambdal*ZZ(2 ,5)+ZZ (4,5)+lambda2*ZZ(5 , 5)] ; 

ZLLD1= [ep*ZZ (2,1) +ep*lambdal *ZZ (2 , 2 ) +ZZ (2 , 4) +lambda2*ZZ (2,5) 
ep*ZZ(3 , 1 ) +ep*lambdal*ZZ (3 , 2) +ZZ (3 , 4) +lambda2*ZZ (3,5) 
ep*ZZ (4,1) +ep*lambdal*ZZ (4 , 2) +ZZ (4 , 4) +lambda2*ZZ (4,5) 
ep*ZZ(5 , 1 ) +ep*lambdal*ZZ(5 , 2) +ZZ (5 , 4) +lambda2*ZZ (5 , 5) ] ; 
WRDl=[ep*WW(l,l)+ep*lambdal*WW(2,l)+WW(4,l)+lambda2*WW(5,l)] ; 

ZRUD 1= [ep*ZZ (1,1) +ep*lambdal*ZZ (2 , 1 ) +ZZ (4 , 1) +lambda2*ZZ (5,1)]; 
ZRLDl=[ep*ZZ(l,l)+ep*lambdal*ZZ(l,2)+ZZ(l,4)+lambda2*ZZ(l,5)] ; 

7 . 7 . CONTINUOUS SECOND DIFFERENTIAL OF CONTROLLAW 7 . 7 , 

WLD2= [ep*lambdal*WW(l ,2)+ep*lambdal“2*WW(2 ,2)+WW(3,2)+ 

lambda2*WW (4, 2)+lambda2~2*WW(5 ,2) ep*lambdal*WW(l ,3)+ 
ep*lambdal~2*WW(2 ,3)+WW(3 ,3)+lambda2*WW(4,3)+ 
lambda2“2*WW(5 ,3) ep*lambdal*WW(l ,4)+ep*lambdal~2* 

WW (2 , 4) +WW (3 , 4) +lambda2*WW (4 , 4) +lambda2~2*WW (5 , 4) 
ep*lambdal*WW(l,5)+ep*lambdal~2*WW(2,5)+WW(3,5)+ 
lambda2*WW(4,5)+lambda2~2*WW(5,5)] ; 

ZLUD2= [ep*lambdal*ZZ (1,2) +ep*lambdal ~2*ZZ(2 , 2) +ZZ (3,2)+ 

lambda2*ZZ(4, 2) +lambda2~2*ZZ(5 , 2) ep*lambdal*ZZ(l , 3)+ 
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ep*lambdal ~2*ZZ (2 , 3) +ZZ (3 , 3) +lambda2*ZZ (4,3)+ 
lambda2~2*ZZ(5,3) ep*lambdal*ZZ(l,4)+ep*lambdal~2* 
ZZ(2,4)+ZZ(3,4)+lambda2*ZZ(4,4)+lambda2“2*ZZ(5,4) 
ep*lambdal*ZZ(l ,5)+ep*lambdal~2*ZZ(2,5)+ZZ(3,5)+ 
lambda2*ZZ(4,5)+lambda2~2*ZZ(5,5)] ; 

ZLLD2=[ep*lambdal*ZZ(2,l)+ep*lambdal‘2*ZZ(2,2)+ZZ(2,3)+ 

lambda2*ZZ(2,4)+lambda2~2*ZZ(2,5) ep*lambdal*ZZ(3, 1)+ 
ep*lambdal“2*ZZ(3,2)+ZZ(3,3)+lambda2*ZZ(3,4)+ 
lambda2“2*ZZ(3,5) ep*lambdal*ZZ(4, l)+ep*lambdal“2* 

ZZ (4 , 2) +ZZ (4 , 3) +lambda2*ZZ (4 , 4) +lambda2“2*ZZ (4 ,5) 
ep*lambdal*ZZ(5, l)+ep*lambdal"2*ZZ(5,2)+ZZ(5,3)+ 
lambda2*ZZ(5 ,4)+lambda2~2*ZZ(5,5)] ; 

WRD2= [ep*lambdal*WW ( 1 , 1 ) +ep*lambdal “ 2*WW(2 , 1)+WW(3 , 1) + 
lambda2*WW(4, l)+lambda2~2*WW(5 , 1)] ; 

ZRUD2=[ep*lambdal*ZZ(l,l)+ep*lambdal~2*ZZ(2,l)+ZZ(3,l)+ 
lambda2*ZZ(4, l)+lambda2~2*ZZ(5, 1 )] ; 

ZRLD2= [ep*lambdal*ZZ(l , l)+ep*lambdal~2*ZZ(l ,2)+ZZ(l ,3)+ 
lambda2*ZZ(l ,4)+lambda2~2*ZZ(l,5)] ; 

Y.7. CONTINUOUS THIRD DIFFERENTIAL OF CONTROLLAW •/,*/. 

WLD3= [ep*lambdal~2*WW(l ,2)+(ep*lambdal“3+l)*WW(2,2)+lambda2* 
WW ( 3 , 2 ) +lambda2 “ 2*WW (4 , 2 ) +1 ambda2 ~3*WW (5,2) 

ep*lambdal“2*WW(l ,3)+(ep*lambdal~3+l)*WW(2,3)+lambda2* 
WW (3 , 3) +lambda2'’2*WW (4 ,3) +lambda2~3*WW(5 , 3) 
ep*lambdal~2*WW(l ,4)+(ep*lambdal~3+l)*WW(2,4)+lambda2* 
WW(3,4)+lambda2~2*WW(4,4)+lambda2''3*WW(5,4) 
ep*lambdal “ 2*WW (1,5) + (ep*lambdal *3+ 1 ) *WW (2 , 5) +lambda2* 
WW(3,5)+lambda2~2*WW(4,5)+lambda2~3*WW(5,5)] ; 

ZLUD3= [ep*lambdal~2*ZZ (1,2)+ (ep*lambdal *3+1) *ZZ(2 , 2) +lambda2* 
ZZ(3,2)+lambda2~2*ZZ(4,2)+lambda2~3*ZZ(5,2) 

ep*lambdal ~2*ZZ (1,3) + (ep*lambdal *3+1) *ZZ (2 , 3) +lambda2* 
ZZ (3 , 3) +lambda2 ~2*ZZ (4 , 3) +lambda2“3*ZZ(5 ,3) 
ep*lambdal~2*ZZ (1,4)+ (ep*lambdal~3+l) *ZZ(2 ,4) +lambda2* 
ZZ (3 , 4) +lambda2~2*ZZ (4, 4) +lambda2“3*ZZ(5 ,4) 
ep*lambdal~2*ZZ (1,5) + (ep*lambdal~3+l) *ZZ(2 , 5) +lambda2* 
ZZ(3,5)+lambda2~2*ZZ(4,5)+lambda2~3*ZZ(5,5)] ; 
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ZLLD3= [ep*lambdal “2*ZZ (2 , 1) + (ep*lambdal “3+1) *ZZ(2 , 2) +lambda2* 
ZZ(2,3)+lambda2“2*ZZ(2,4)+lambda2“3*ZZ(2,5) 
ep*lambdal“2*ZZ(3,l)+(ep*lambdal“3+l)*ZZ(3,2)+lambda2* 
ZZ(3,3)+lambda2“2*ZZ(3,4)+lambda2“3*ZZ(3,5) 
ep*lambdal“2*ZZ(4, l)+(ep*lambdal“3+l)*ZZ(4,2)+lambda2* 
ZZ(4,3)+lambda2~2*ZZ(4,4)+lambda2‘3*ZZ(4,5) 
ep*lambdal“2*ZZ(5 , 1) + (ep*lambdal“3+l) *ZZ (5 , 2) +lambda2* 
ZZ(5,3)+lambda2“2*ZZ(5,4)+lambda2“3*ZZ(5,5)] ; 
WRD3=[ep*lambdal“2*WW(l,l)+(ep*lambdal“3+l)*WW(2,l)+lainbda2* 
WW(3,l)+lambda2“2*WW(4,l)+lambda2“3*WW(5,l)] ; 

ZRUD3= [ep*lambdal “2*ZZ (1,1)+ (ep*lambdal “3+1) *ZZ(2 , 1) +lambda2* 
ZZ(3,l)+lambda2“2*ZZ(4,l)+lambda2“3*ZZ(5,l)] ; 

ZRLD3= [ep*lambdal“2*ZZ ( 1 , 1) + (ep*lambdal“3+l) *ZZ(1 , 2) +lambda2* 
ZZ(1 ,3)+lambda2“2*ZZ(l ,4)+lambda2‘3*ZZ(l ,5)] ; 

7.7. DIFFERENTIAL APPROXIMATIONS FOR THE BOUNDARY CONDITIONS 7.7. 

ydlO=(R(l,2)-R(l,l))/h; 
ydll=(R(l ,3)-R(l ,2) )/h; 
ydl2=(R(l ,4)-R(l ,3) )/h; 
ydl3=(R(l ,5)-R(l ,4) )/h; 
ydln=(R(l ,n+l)-R(l ,n))/h; 
ydln_l=(R(l ,n)-R(l ,n-l) )/h; 
ydln_2=(R(l ,n-l)-R(l ,n-2))/h; 
ydln_3=(R(l,n-2)-R(l,n-3))/h; 
uO= (ydlO-ydl 1) /h ; 
ul=(ydll-ydl2)/h; 
u2=(ydl2-ydl3)/h; 
un= (ydln_ 1-ydln) /h ; 
un_l=(ydln_2-ydln_l)/h; 
un_2= (ydln_3-ydln_2) /h ; 
udlO=(uO-ul)/h; 
udll=(ul-u2)/h; 
udln= (un_ 1-un) /h ; 
udln_l=(un_2-un_l)/h; 
ud20=(udl0-udll)/h; 
ud2n= (udln_ 1-udln) /h ; 
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*/%*/% 4/5 of the the first state vector */%*!, 

X0= [ydlO ; uO; udlO; ud20] ; 

*1*1% 4/5 of the the last state vector *1*1% 

Xn=[ydln; un; udln; ud2n] ; 

*/.*/. FORMING OF THE RIGHT HAND SIDE OF THE 
BLOCKDIAGONAL SYSTEM VI, 

j=i; 

for i=2:n 

Omega (j , 1)=-ZRLD0*R( 1 , i-1) +WRD0*R(1 , i)-ZRUD0*R(l , i+1) ; 

j=j+i; 

Omega (j , 1)=-ZRLD1*R(1 , i-l)+WRDl*R(l , i)-ZRUDl*R(l , i+1 ) ; 

j=j + i; 

Omega ( j , 1 ) =-ZRLD2*R ( 1 , i " 1 ) +WRD2*R ( 1 , i ) -ZRUD2*R( 1 , i+ 1 ) ; 

j=j+i; 

Omega ( j , 1 ) =-ZRLD3*R( 1 , i - 1 ) +WRD3*R ( 1 , i) -ZRUD3*R( 1 , i+ 1 ) ; 

j=j+i; 

end; 

Omega (1,1) =Omega (1,1) -ZLLD0*X0 ; 

Omega (2, l)=0mega(2, 1)-ZLLD1*X0; 

Omega (3,1) =Omega (3 , 1 ) -ZLLD2*X0 ; 

Omega (4 , 1 ) =Omega (4 , 1)-ZLLD3*X0; 

Omega(4*n-7 , 1) =0mega(4*n-7 , 1) -ZLUDO*Xn; 

0mega(4*n-6, l)=0mega(4*n-6, l)-ZLUDl*Xn; 

Omega (4*n-5 , 1) =0mega (4*n-5 , 1 ) -ZLUD2*Xn ; 
Omega(4*n-4,l)=Omega(4*n-4,l)-ZLUD3*Xn; 

•/.*/. FORMING OF THE LEFT HAND SIDE OF THE 
BLOCKDIAGONAL SYSTEM */,•/, 

for i=l:4 

DD ( 1 , i ) =- WLDO ( 1 , i ) ; 

DD ( 2 , i ) = - WLD 1 ( 1 , i ) ; 

DD(3, i)=-WLD2(l ,i) ; 

DD(4, i)=-WLD3(l , i) ; 
if n>2 
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DD ( 1 , i+4) =ZLUD0 ( 1 , i ) ; 

DD(2 , i+4)=ZLUDl ( 1 , i) ; 

DD (3 , i+4) =ZLUD2 ( 1 , i ) ; 

DD(4,i+4)=ZLUD3(l,i) ; 
end; 
end; 

for i=2 : n-2 
for j — l:2 

DD (4*i-3,4*i-6+j ) =ZLLD0 ( 1 , j +2) ; 
DD(4*i-3,4*i-2+j)=-WLD0(l, j+2); 
DD(4*i-3,4*i+2+j)=ZLUD0(l, j+2); 

DD(4*i-2 ,4*i-6+j )=ZLLD1 (1 , j +2) ; 
DD(4*i-2,4*i-2+j)=-WLDl(l, j+2) ; 

DD(4*i-2 ,4*i+2+j )=ZLUD1 (1 , j+2) ; 

DD(4*i-l ,4*i-6+j )=ZLLD2(l , j+2) ; 

DD (4*i- 1 , 4*i-2+ j ) =-WLD2 ( 1 , j +2) ; 

DD (4*i-l ,4*i+2+j ) =ZLUD2 ( 1 , j +2) ; 

DD(4*i , 4*i-6+j)=ZLLD3(l, j+2) ; 

DD(4*i, 4*i-2+j )=-WLD3(l , j+2) ; 

DD(4*i , 4*i+2+j ) =ZLUD3(l , j+2) ; 

end; 
end; 
if n>2 
for i=l:4 

DD(4*n-7,4*n-12+i)»ZLLD0(l,i) ; 
DD(4*n-6,4*n-12+i)=ZLLDl (1 , i) ; 
DD(4*n-5 J 4*n-12+i)=ZLLD2(l,i) ; 
DD(4*n-4,4*n-12+i)=ZLLD3(l , i) ; 

DD (4*n-7 ,4*n-8+i)=-WLD0 ( 1 , i) ; 
DD(4*n-6,4*n-8+i)=-WLDl(l,i) ; 

DD (4*n-5 ,4*n-8+i)=-WLD2 ( 1 , i) ; 

DD (4*n-4 , 4*n-8+i) =-WLD3 ( 1 , i ) ; 
end; 
end; 

DD=sparse(DD) ; •/,•/, SQUEEZING OUT ALL ZERO ELEMENTS 

FROM MATRIX DD %7. 



10 MATLAB PROGRAMS 


91 


*/.*/. GAUSSELIMINATION TD PRODUCE AN UPPER TRIANGULAR SYSTEM •/.'/. 

for k=2:4:4*n-10 
for l=k:k+6 

if DD(k-l ,k-l)~=0 

zd=DD(l ,k-l)/DD(k-l ,k-l) ; 

DD(1, :)=DD(1, :)-zd*DD(k-l, :); 

OmegaCl, l)=OmegaCl, l)-zd*Omega(k-l,l) ; 
end; 
end; 

for l=k+l :k+6 
if DD(k,k)*=0 
zd=DD(l ,k)/DD(k,k) ; 

DD(1, :)=DD(1, :)-zd*DD(k, :); 

Omega(l, l)=0mega(l, 1) -zd*0mega(k, 1) ; 
end; 
end; 

for l=k+2:k+6 

if DD(k+l ,k+l)~=0 
zd=DD(l ,k+l)/DD(k+l ,k+l) ; 

DD(1, :)=DD(1, :)-zd*DD(k+l, :); 

OmegaCl, l)=0megaCl, 1) -zd*OmegaCk+l , 1) ; 
end; 
end; 

for l=k+3:k+6 

if DDCk+2,k+2)"=0 
zd=DDCl,k+2)/DDCk+2,k+2) ; 

DD Cl , : ) =DD C 1 , : ) -zd*DD Ck+2 , : ) ; 

OmegaCl , 1) =0megaCl , 1) -zd*0megaCk+2, 1) ; 
end; 
end; 
end; 

k=4*n-7 ; 
if DDCk,k)~=0 
zd=DDCk+l,k)/DDCk,k) ; 

DDCk+1, :)=DDCk+l, :)-zd*DDCk, :); 
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Omega(k+l , l)=Omega(k+l , l)-zd*Omega(k, 1) ; 
zd=DD (k+2 , k) /DD (k , k) ; 

DD (k+2 , : ) =DD (k+2 , : ) -zd*DD(k, : ) ; 

Omega (k+2 , l)=0mega(k+2, 1) -zd*Omega(k, 1) ; 
zd=DD (k+3 , k) /DD (k , k) ; 

DD(k+3, :)=DD(k+3, :)-zd*DD(k, :); 

0mega(k+3, l)=0mega(k+3, l)-zd*Omega(k,l) ; 
end; 

if DD(k+l,k+l)~=0 

zd=DD(k+2,k+l)/DD(k+l ,k+l) ; 

DD(k+2 , : )=DD(k+2, : )-zd*DD(k+l , : ) ; 

0mega(k+2 , l)=0mega(k+2, 1) -zd*Omega(k+l, 1) ; 
zd=DD(k+3,k+l)/DD(k+l,k+l) ; 

DD (k+3 , : ) =DD (k+3 , : )-zd*DD(k+l , : ) ; 
Omega(k+3, l)=0mega(k+3, l)-zd*Omega(k+l, 1) ; 
end; 

if DD(k+2,k+2)'=0 

zd=DD (k+3 , k+2) /DD (k+2 , k+2) ; 

DD (k+3 , : )=DD (k+3 , : ) -zd*DD (k+2 , : ) ; 
0mega(k+3, l)=0mega(k+3, l)-zd*0mega(k+2, 1) ; 
end; 


BACKSUBSTITUTION TO SOLVE FOR THE STATEVECTORS '/.*/. 

ud2(n-l)=Omega(4*n-4, l)/DD(4*n-4,4*n-4) ; 

udl (n-l)=(0mega(4*n-5 , l)-DD(4*n-5 ,4*n-4)*ud2(n-l))/ 

DD(4*n-5,4*n-5) ; 

u(n-l)=( Omega (4*n-6 , 1 ) -DD (4*n-6 , 4*n-4) *ud2 (n- 1 ) - 

DD (4*n-6 , 4*n-5)*udl (n- 1 ) ) /DD (4*n-6 , 4*n-6) ; 

ydl (n- 1 ) = (Omega(4*n-7 , 1 ) -DD (4*n-7 ,4*n-4) *ud2 (n- 1) - 

DD (4*n-7 , 4*n-5 ) *udl (n- 1 ) -DD (4*n-7 ,4*n-6) *u (n-1) ) / 

DD(4*n-7,4*n-7) ; 

if n>2 

for k=n-2 : -1 : 1 

ud2(k)=( Omega (4*k , 1 ) -DD (4*k , 4*k+4) *ud2 (k+1 ) - 
DD (4*k , 4*k+3 ) *ud 1 (k+ 1 ) -DD (4*k , 4*k+2) *u (k+1 ) - 
DD (4*k , 4*k+l) *ydl (k+ 1 ) ) /DD (4*k , 4*k) ; 
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udl (k)=( Omega (4*k-l , l)-DD(4*k-l ,4*k+4)*ud2(k+l)- 
DD (4*k- 1 , 4*k+3) *ud 1 (k+ 1 ) -DD (4*k- 1 , 4*k+2) *u (k+ 1 ) - 
DD (4*k- 1 , 4*k+ 1 ) *yd 1 (k+ 1 ) -DD (4*k- 1 , 4*k) *ud2 (k) ) / 
DD (4*k- 1 , 4*k- 1 ) ; 

u(k) - (0mega(4*k-2 , 1) -DD (4*k-2 , 4*k+4) *ud2 (k+1) - 
DD(4*k-2,4*k+3)*udl(k+l)-DD(4*k-2,4*k+2)*u(k+l)- 
DD (4*k-2 , 4*k+ 1 ) *yd 1 (k+ 1 ) -DD (4*k-2 , 4*k) *ud2 (k) - 
DD (4*k-2 , 4*k- 1) *udl (k) ) /DD (4*k-2 , 4*k-2 ) ; 
ydl (k) = ( Omega (4*k-3 , 1 ) -DD (4*k-3 , 4*k+4) *ud2 (k+ 1 ) - 
DD (4*k-3 , 4*k+3) *udl (k+ 1 ) -DD (4*k-3 , 4*k+2) *u (k+ 1 ) - 
DD (4*k-3 , 4*k+ 1 ) *ydl (k+ 1 ) -DD (4*k-3 , 4*k) *ud2 (k) - 
DD(4*k-3 ,4*k-l)*udl (k) -DD(4*k-3 ,4*k-2)*u(k) )/ 
DD(4*k-3 ,4*k-3) ; 
end; 
end; 


•/.*/. MAKING OF THE STATEVECTORS It 

x(:,l)-[R(l,l); XO] ; 
x(: ,n+l)»[R(l,n+l) ; Xn] ; 
for i=l:n-l 

x(: , i+l)= [R(l , i+1) ; ydl(i); u(i); udl(i); ud2(i)] ; 
end; 

if cleargr 
elf ; 

hold on; 
grid on; 
end; 


*/.*/. PLOTTING OF THE CALC/SPEC TRAJECTORY, VELOCITY, 
CONTROLS IGNAL AND ACCELERATION •/.*/. 


plotadm=0 ; 
while spc_plot 
if plotadm 

if spc_plot<12 
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figure; 
hold on; 
grid on; 

title( [’One dim. traj : mprl51knovel .m 11 = 

’ ,num2str(lambdal) , ’ 12 = ' ,num2str(lambda2) , ' ep = 

’ ,num2str(ep) , ’ n = ’ ,num2str(n)] ) 

xlabel( ['Total error : ’ ,num2str(Total_error) , 

’ Point error : ’ ,num2str (Point .error) , ’ Graph : 
’ ,num2str(spc_plot) , ’ Time t’]) 
if spc_plot<6 
for k=0:n 

plot(k*h,x(l,k+l) , ’o’) 
end; 
end; 

end; 

end; 

breakadm=0; 

fadm=0; */.•/. NORMALLY fadm=0, NECESSARY FOR WRITING OF 
TEXTS IN THE PLOT •/.*/. 

for j=0:m 

eAtau=expm(A*j*h/m) ; 

for i=f adm:n-l */.*/. CHOOSE e.g 2:n-3 TO AVOID 

PROBLEMS AT THE ENDPOINTS 11 
entry( : , i+l)=eAtau*(x( : ,i+l)+Mtau(: ,5*j+l :5*j+5)* 
Minv*(e_Ah*x( : , i+2)-x( : , i+1)) ) ; 
csignvec( : , i+l)=B’*expm(-A’ *j *h/m)*Minv* 

(e_Ah*x( : , i+2)-x( : , i+1) ) ; 
if j==0 

if i==fadm; 

entryl=entry(l,fadm+l) ; 
entry2=entry(2,fadm+l) ; 
entry3=entry (3,fadm+l) ; 
entry4=entry (4,fadm+l) ; 
entry5=entry(5,fadm+l) ; 
csignvecl=csignvec(l , 1) ; 
end; 0 
end; 
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if rem(j ,mp)==0 
if j<=m-mp 

traject (1 , j/mp*n+(i+l))=entry(l, i+1) ; 
end; 
end; 
if j ==m 
if i==n-l 

traject(l,6*n+l)=entry(l,i+l) ; 
end; 
end; 

if spc_plot==l 

plot (i*h+j*h/m, entry (1 , i+1) ) VI. TRAJECTORY 7.7. 

plot (i*h+j*h/m, entry (3, i+1) , ’ . ’) V/. CONTROL u VI. 

plot (i*h+j*h/ra , lambdal*entry (2 , i+l)+entry (3, i+l)+ 
ep*csignvec ( 1 , i+1) , ; . * ) 7.7. ACCELERATION VI. 

elseif spc_plot==2 

plot(i*h+j*h/m,entry(l,i+l) , ’ . ') VI. TRAJECTORY VI. 
plot(i*h+j*h/m,entry(2, i+1) , ’ . ') VI. VELOCITY 
plot (i*h+j*h/m, entry (3, i+1) ) VI. CONTROL u VI. 

plot (i*h+j*h/m, lambdal*entry(2, i+l)+entry(3, i+l)+ 
ep*csignvec(l , i+1) , ' . ’ ) VI. ACCELERATION Vf. 
elseif spc_plot==3 

plot ( i*h+ j *h/m , entry (1 , i+1) , ' . » ) 7.7. TRAJECTORY 7.7. 

plot (i*h+j*h/m, lajnbdal*entry(2, i+l)+entry(3, i+l) + 
ep*csignvec(l , i+1) , ' . ' ) VI. ACCELERATION VI. 
elseif spc_plot==4 

plot(i*h+j*h/m,entry(l,i+l) , ’ . ') VI. TRAJECTORY 7.7. 
plot (i*h+j*h/m, entry (2, i+1) ') VI. VELOCITY 7.7. 
elseif spc_plot==5 

plot (i*h+j*h/m, entry (1 , i+1) ) VI. TRAJECTORY 7.7. 

plot(i*h+j*h/m,entry(3,i+l) , ' . ’) VI. CONTROL u 7.7. 
elseif spc_plot==6 

7.7. CONTROL DER u-dot 7.7. 
plot (i*h+j*h/m, entry (4, i+1) , ' . ') 
elseif spc_plot==7 

7.7. CONTROL DERx2 u-dot-dot 7.7. 
plot (i*h+j*h/m, entry (5, i+1) , ’ . ') 
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elseif spc_plot==8 

csignvec( : ,i+l)=B'*expm(-A , *j*h/m)*Minv* 
(e_Ah*x( : ,i+2)-x( : , i+1) ) ; 

*/.*/. CONTROLS IGNAL w 7.7. 
plot(i*h+j*h/m,csignvec(l ,i+l) , ’ . 
elseif spc_plot==9 

csdlvec( : ,i+l)=B'*A , *expm(-A'*j*h/m)*Minv* 
(e_Ah*x( : ,i+2)-x( : , i+1)) ; 

U CONTROL DER w-dot V/, 
plot(i*h+j*h/m,csdlvec(l,i+l) , ’ . ') 
if j ==0 

csdvecl=csdlvec(l , 1) ; 
end; 

elseif spc_plot==10 

csd2vec( : ,i+l)=B , *A , *A , *expm(-A’*j*h/m)*Minv* 
(e_Ah*x( : ,i+2)-x( : ,i+l)) ; 

•/.*/. CONTROL DERx2 w-dot-dot Y.7. 
plot(i*h+j*h/m,csd2vec(l,i+l) , ’ . ') 
if j ==0 

csdvec2=csd2vec(l , 1) ; 
end; 

elseif spc_plot==ll 

csd3vec( : , i+l)=B ' *A' *A ’ *A ’ *expm(-A’*j*h/m)* 
Minv*(e_Ah*x( : , i+2)-x( : ,i+l) ) ; 

•/,*/. CONTROL DERx3 v-dot-dot-dot 7.7, 
plot (i*h+j*h/m,csd3vec(l , i+1) , ' . ' ) 
if j==0 

csdvec3=csd3vec(l , 1) ; 
end; 
else 

disp(' ’ ) 

disp( ' NOT A VALID CHOICE ') 
breaLkadm=l ; 
end; 

if breakadm 
break; 
end; 
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end; 

if breakadm 
break; 
end; 
end; 

if spc_plot<12 
ax=axis ; 
ax2=ax(l,2) ; 
if ax2<1.5 
xpos=3/4*0.2; 
elseif ax2>=5 
xpos=3/4; 
else 

xpos=0 . 25; 
end; 

if spc_plot<3 
ax4=ax(l ,4) ; 
if ax4<l 
ax4=ax4*10; 
if ax4<l 
ax4=ax4*10; 
end ; 
end; 

if ax4>10 
ax4=ax4/10; 
if ax4>10 
ax4=ax4/10; 
end; 
end; 

if rem(ax4,2)==0 

yadd=(ajc(l ,4)/4)*l/5 ; 
else 

yadd=(ax(l ,4)/3)*l/5 ; 
end; 

ypos=lambdal*entry2+entry3+ep*csignvecl ; 
text(-xpos,ypos, ['ACC']) ; 
if abs(ypos-entry3)>yadd 
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text (-xpos,entry3, [’CS u’]); 
elseif ypos-entry3>0 

text (-xpos , entry3-yadd , [ ' CS u ’ ] ) ; 
else 

text (-xpos, entry3+yadd, [’CS u’]) ; 
end; 

if spc_plot==2 

text (-xpos, entry2, [’VEL’] ) ; 
end; 
end; 

if spc_plot==3 

ypos=lambdal*entry2+entry3+ep*csignvecl ; 
text (-xpos, ypos, ['ACC’]) ; 
end; 

if spc_plot==4 

text (-xpos, entry2, ['VEL’]) ; 
end; 

if spc_plot==5 

text (-xpos, entry3, [’CS u’]); 
end; 

if spc_plot==6 

text (-xpos, entry4, ['udl'] ) ; 
end; 

if spc_plot==7 

text (-xpos, entry5, ['ud2']) ; 
end ; 

if spc_plot==8 

text (-xpos, csignvecl, [’CS w’]); 
end; 

if spc_plot==9 

text (-xpos, csdvecl, ['wdl']) ; 
end; 

if spc_plot==10 

text (-xpos , csdvec2, [’ wd2 '] ) ; 
end; 

if spc_plot==ll 

text (-xpos, csdvec3, ['wd3']) ; 
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end; 

end; 

•/.*/, ESTIMATION OF THE ACCURACY IN THE 
SPLINE APPROXIMATION VI. 

if plotadm==0 

[Total_error , Average_error , Point _error ,End_point_error] = 
spline_error (pointf cn,R,traject) ; 

Total_error 

Average_error 

Point_error 

End_point_error 

title(['0ne dim. traj : mprl51knovel .m 11 = 

' ,num2str(lambdal) , ' 12 = ' ,num2str(lambda2) , ' ep = 

’ ,num2str(ep) , ' n - ' ,num2str(n)] ) 

xlabel( [' Total error : ' ,num2str (Total_error) , 

' Point error : ' ,num2str(Point_error) , ' Graph : 

’ ,num2str(spc_plot) , ' Time t']) 
if spc_plot<6 
for k=0:n 

plot(k*h,x(l ,k+l) , 'o') 
end; 
end; 
end; 

plotadm=plotadm+l ; 
disp(' ') 

disp( ' FOLLOWING OPTIONS ARE AVAILABLE : ') 
disp(' ') 

disp( ' FINISH THE PROGRAM : 0 ') 

disp( ’ DISPLAY THE TRAJECTORY, CONTROL u AND 

ACCELERATION : 1 ') 

disp( ' DISPLAY THE TRAJECTORY, VELOCITY, CONTROL u AND 
ACCELERATION : 2 ') 

disp( ' DISPLAY THE TRAJECTORY AND ACCELERATION : 3 ') 
disp(' DISPLAY THE TRAJECTORY AND VELOCITY : 4 ') 
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disp(' DISPLAY THE TRAJECTORY AND CONTROL u : 5 ’) 
disp(' DISPLAY THE FIRST DERIVATIVE OF THE CONTROL u : 6 ') 
disp( ' DISPLAY THE SECOND DERIVATIVE OF THE 
CONTROL u : 7 ') 

disp( ' DISPLAY THE CONTROLSIGNAL w : 8 ') 

disp( ' DISPLAY THE FIRST DERIVATIVE OF THE CONTROL w : 9 ') 
disp( ’ DISPLAY THE SECOND DERIVATIVE OF THE 
CONTROL w : 10 ') 

disp( ' DISPLAY THE THIRD DERIVATIVE OF THE 
CONTROL v : 11 ') 

spc_plot = input ( ’ MAKE YOUR CHOICE : '); 
end; 


clear global; 
for k=2:plotadm 
delete(k) ; 
end; 
end; 


function [Q,cnt] = quad815mod(funfcn,a,b,tol) 

^Alteration of the original matlab toolbox program. 

XQUAD8 Numerical evaluation of an integral, higher order 
7 . method. Q = QUAD8( ’F' ,A,B,T0L) approximates the 
7 % integral of F(X) from to B to within a relative error 

7 , of TOL. J F' is a string containing the name of the 

7 % function. The function must return a 5*5-matrix 
% output value if given an input value. 

7 % Q * Inf is returned if an excessive recursion level 
*/. is reached indicating a possibly singular integral. 

7 % QUAD8 uses an adaptive recursive Newton Cotes 8 panel 

rule . 

7 , Cleve Moler, 5-08-88. 

*/, Copyright (c) 1984-94 by The MathWorks, Inc. 

7 , [Q,cnt] = quad8(F,a,b,tol) also returns a function 
7 , evaluation count. 

*/. Top level initialization, Newton-Cotes weights 
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w = [3956 23552 -3712 41984 -18160 41984 -3712 23552 
3956]/14175; 

x = a + (0:8)* (b-a)/8 ; 

'/. set up function call 
for i=x 

y - [y feval(funfcn, i)] ; 
end; 

/, Adaptive, recursive Newton-Cotes 8 panel quadrature 
Q0 = zeros(5) ; 

[Q , cnt] = quad815stpmod(funfcn,a,b,tol,0,w,x,y,Q0) ; 

cnt = cnt + 9; 

end; 


function [Q , cnt] - quad815stpmod(FunFcn , a,b,tol »lev, 

w,x0,f0,Q0) 

/Alteration of the original matlab toolbox program. 
•/•QUAD8STP Recursive function used by QUAD8. 

*/• [Q,cnt] * quad8stp(F,a,b,tol,lev,w,f ,Q0) tries to 
/ approximate the integral of f(x) from a to b to 
/ within a relative error of tol. F is a string 
/ containing the name of f . The remaining arguments 
/ are generated by quad8mod or by the recursion. 

X lev is the recursion level. 

/ w is the weights in the 8 panel Newton Cotes formula. 

*/* x0 is a vector of 9 equally spaced abscissa is the 

X interval. 

’/• f0 is a matrix of the 9 function values at x. 

*/ QO is an approximate value of the integral. 

*/ Cleve Moler, 5-08-88. 

*/ Copyright (c) 1984-94 by The MathWorks, Inc. 

LEVMAX - 10 ; 

X Evaluate function at midpoints of left and 
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right half intervals, 
x - zeros(l , 17) ; 
x(l : 2 : 17) = xO; 

x(2 : 2 : 16) = (x0(l:8) + x0(2:9))/2; 

f ( : , 1:5)* f0(: ,1:5) ; 
for i=l:8 

f ( : , 10*i-4 : 10*i) = feval(FunFcn,x(2*i)) ; 
f (: ,10*i+l:10*i+5) = (f 0( : ,5*i+l : 5*i+5)) ; 
end; 

7, Integrate over half intervals, 
h = (b-a)/16; 

Q1=0 ; Q2=0 ; 

for i=l:9 

qi = qi + h*w(i)*f (: ,5*i-4:5*i) ; 
q2 = q2 + h*w(10-i)*f (: ,86-i*5 : 90-i*5) ; 
end; 

q = qi + q 2 ; 

7,7, Recursively refine approximations, 
if norm(q - qo) > tol*norm(q) k lev <= LEVMAX 
c = (a+b)/2; 

[qi.cntl] = quad815stpmod(FunFcn,a,c,tol/2,lev+l, 

w,x(l :9) ,f (: ,1:45) ,Q1) ; 
[q2,cnt2] * quad815stpmod(FunFcn,c,b,tol/2,lev+l, 

v,x(9:17),f(:,41:85),q2); 

Q = QI + Q2; 

cnt = cnt + cntl + cnt2; 

end 

end; 
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function res = integrand(v) 

y.y. THIS SUBPROGRAM DETERMINE THE INTEGRAND OF THE MATRIX 
CONSTANT M 

global A B 

e_AvB=expm(-A*v) *B; 
res = e_AvB*e_AvB’ ; 
end; 


function [Tot al_error , Average_error , Point .error , 

End.point .error] =spline_error(pointfcn,R, traj ect) 

global h t n 

Total.error=0; 

Point_error=0 ; 
n=6*n; 

Rp=f eval(pointfcn) ; 
n=n/ 6 ; 
h=t/n; 
for i=0:n-l 
for j=0:5 

Total_error=Total_error+abs(traject (1, j*n+l+i)- 

Rp(l,i*6+j+l)) ; 
if j ==0 

Point _error=Point_error+abs (traj ect (1 , i+l)-R(l , i+l) ) ; 
end; 
end; 
end ; 

Average_error=Total_error/ (6*n) ; 

End_point_error=abs (traj ect ( 1 , 6*n+l) -R(l ,n+l) ) ; 
end ; 
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function R=pointexpll 
% R = l*(n+l) -matrix, 
global h t n 

'/,*/, TIME INTERVAL FOR RENEWAL OF THE TRAJECTORY '/.*/. 
h=t/n; 

for j=0 :h :n*h 

R(l,j/h+l)=(l-exp(-3/2*(j-t/2)))/(l+exp(-3/2*(j-t/2))); 

end; 

end; 


function R=pointsinl2 
*/. R = l*(n+l) -matrix, 
global h t n 

*/."/. TIME INTERVAL FOR RENEWAL OF THE TRAJECTORY VI, 
h=t/n; 

*/,*/, I APOLOGIZE FOR THE "SMART" PROGRAMING It 
adm=n*3/26; 
for j = 1 : adm 
R(l,j)=0; 
end; 

for j=0 :h: (n-2*adm)*h 

R(l, j/h+adm+l)=l/l0*(l+sin(-pi/2+j*pi/2)); 
end; 

for j = 1 : adm 

R(l,j-adm+n+l)=0; 

end; 

end; 
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function R=pointstepl 
y. R = l*(n+l) -matrix, 
global h t n 
h=t/n; 

for j=0:h:n*h 

R(l , j /h+l) = l/2* (l +si 6 I1 (j - Ct/ 2+0 • °i)) ) ; 
end; 
end; 
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